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ABSTRACT 

\  - 

\ 

It  is  shown  that  Kalman  filtering  may  be  applied  to  the  radar  track- 
while-scan  problem.  No  attempt  is  made  to  rigorously  derive  the  Kalman 
equations,  but  the  equations  are  related  to  more  familiar  ideas. 

It  is  demonstrated  that  the  least  squares  oB  equations  constitute 
a  special  case  of  the  Kalman  filter.  The  approach  used,  however,  does 
not  require  a  constant  data  rate  or  constant  measurement  accuracy,  meaning 
that  information  from  various  sensors  (including  links)  nay  be  used. 


ADMIRALTY  SURFACE  WEAPONS  ESTABLISHMENT 
PORTSMOUTH  HANTS 


LIST  OF  CONTENTS 


APPENDIX  A  EQUIVALENCE  OF  KALMAN  FILTER,  LEAST  SQUARES  a0  TRACKERS 
AND  LINEAR  REGRESSION,  FOR  SINGLE  DIMENSION  CONSTANT 
ERROR  CONSTANT  DATA  RATE  CONDITIONS 

APPENDIX  B  TRANSFORMATION  OF  VECTORS  AND  THEIR  COVARIANCES 

APPENDIX  C  ESTIMATION  OF  MOST  PROBABLE  POSITION 

APPENDIX  D  APPLICATION  OF  RESULTS  IN  APPENDIX  B  IN  POLAR  TO 
CARTESIAN  TRANSFORMATION 

APPENDIX  E  XY  FIRST  ORDER  TRACKER  -  PROGRAM  HARD  COPY 
APPENDIX  F  POLAR  FIRST  ORDER  TRACKER  -  PROGRAM  HARD  COPY 
APPENDIX  G  POLAR  SECOND  ORDER  TRACKER  -  PROGRAM  HARD  COPY 


LIST  OF  ILLUSTRATIONS 


Figure  1 
Figure  2 
Figure  3 
Figure  4 
Figure  5 
Figure  6 
Figure  7 
Figure  8 
Figure  9 
Figure  10 
Figure  11 
Figure  12 
Figure  13 
Figure  14 
Figure  15 


Response  to  a  turn  with  noise  free  data. 

1st  Order  Kalman  (R,0,R,0) 

Second  Order  Polar  Kalman  Filter 
XY  Kalman  Tracker  (First  Order) 

First  Order  Cartesian  Tracker 
First  Order  Polar  Kalman 
First  Order  R0  Kalman 
Second  Order  R0  Kalman  Tracker 
XY  Kalman  Tracker 

X-Errors  From  Figure  9  and  their  a  priori  standard  deviations 

Y-Errors  from  Figure  9  and  their  a  priori  standard  deviations 

1st  Order  R0  Kalman  Tracker 

First  Order  R0  Kalman  Tracker 

Second  Order  RO  Kalman  Tracker 

Second  Order  R0  Kalman  Tracker 

A  W 


(iii) 


INTRODUCTION 

Ttlis  report  is  intended  to  be  the  first  of  a  series  resulting  from  a  study  on 
ie  gjneral  subjects  of  target  tracking  and  association.  The  purpose  of  the  study 
is  to  prodn  ;e  a  generalised  package,  a  subset  of  which  would  be  chosen  for  any 
particular  system  with  any  particular  combination  of  sensors.  Alternatively,  the 
general >ied  package  could  be  used  as  a  standard  with  which  to  compare  simplified 
processes. 

2.  The  aim  of  the  first  phase  of  the  work  is  to  produce  a  generalised  tracking 
system  capable  of  using  elevation  and  doppler  information,  as  well  as  having  a 
predictable  performance  against  manoeuvring  vehicles.  Because  this  present  work 
does  not  include  any  turn  handling  capabilities,  it  is  not  considered  a  viable 
process  on  its  own,  although  some  people  have  used  such  a  method  on  its  own  with 
limited  success  in  other  applications. 

3.  Many  of  the  ideas  presented  may  be  expressed  in  a  considerably  simpler  way  by 
using  some  matrix  theorems.  This  has  been  avoided  since  the  author  assumes  very 
little  knowledge  of  matrix  algebra  on  the  part  of  the  reader;  one  purpose  of  the 
report  being  to  indicate  the  benefits  of  this  approach. 

4.  This  report  is  thus  concerned  with  deriving  equations  of  motion  of  tracks  from 
information  from  imperfect  sensors,  and  minimising  the  noise  on  these  tracks.  This 
report  considers  only  two  dimensional  applications. 

5.  It  is  shown  how  information  from  a  one -dimensional  sensor  may  be  incorporated. 
WHY  USE  A  COMPUTER  TO  FORM  TRACKS  FROM  SURVEILLANCE  INFORMATION? 

6.  We  need  to  remind  ourselves  of  the  answers  to  such  fundamental  questions  in 
order  to  keep  sight  of  our  objectives.  The  author  sees  three  basic  reasons. 

a.  To  provide  a  tactical  picture 

A  display  of  the  tracks  of  all  vehicles  observed  by  sensors  showing  present 
positions,  courses  and  speeds  etc  is  essential  for  the  deployment  of  a  ship  and 
its  weapons.  The  computer  enables  rapid  use  to  be  made  of  sensor  information 
thereby  ensuring  that  the  picture  is  accurate  and  up  to  data. 

b.  For  use  in  processes  such  as  threat  evaluation  and  weapon  assignment. 

The  computer  can  forecast  future  likely  positions  of  tracked  vehicles  [and  rapidly 
perform  necessary  calculations  to  assist  operators  in  the  assessment  of  threats 
and  in  the  optimum  weapon  deployment  to  deal  with  them. 

c.  To  provide  target  information  for  weapon  deployment. 

he  computer  can  be  used  to  generate  smooth  tracks  from  noisy  information  thereby 
being  able  to  pass  information  to  a  weapon  sensor  more  accurately  than  information 
derived  from  a  single  plot  on  a  display.  The  computer  may  also  provide  continuous 
information  obtained  by  continually  increasing  the  extent  of  the  extrapolation 
from  its  previous  best  estimate  of  target  position. 

The  third  roason  is  generally  thought  to  specify  the  most  stringent  requirement. 
However,  association  processes  usually  demand  the  most  of  the  tracking. 

7.  Thus  the  uses  of  the  computer  are  basically  to  provide  quick  calculations 
using  input  data  and  to  remove  the  tedium  from  man's  task  in  tracking,  ie  taking 
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care  of  most  tracks  (and  possibly  new  detections),  while  the  man  would  be  able  to 
concentrate  on  the  overall  situation.  The  man  would  also  need  to  handle  those 
situations  at  which  the  computer  is  less  capable;  such  situations  would  generally 
be  ones  which  require  the  use  of  qualitative  information  for  their  solutions. 


SUMMARY  OF  PRESENT  METHODS 


8. 


a.  The  traditional  method  (known  as  ci-8  tracking)  is  well  documented  (for 
example  see  Reference  1-4). 

The  method  is  basically  as  follows:  a  forecast  position  of  the  vehicle  being 
tracked  is  calculated,  and  a  fraction  of  the  difference  between  this  forecast 
and  the  observed  positions  is  added  to  the  forecast  to  give  an  estimate  of 
true  position  at  the  time  of  the  observation.  The  assumed  velocity  is 
incremented  by  a  fraction  of  the  difference  between  observed  and  forecast 
positions,  divided  by  the  time  between  observations.  The  next  forecast 
position  is  then  the  present  estimate  of  position  extrapolated  using  the 
estimated  velocity.  The  process  is  started  by  calculating  a  velocity  from  the 
first  two  observations,  and  using  this  to  forecast  from  the  second  observed 
position. 


The  equations  are  thus : 


G  =  F  +  a  (P  -F  ) 
n  n  n  n' 


V  =  V  .  +  £  (P  -F  ) 
n  n-1  T  n  n 


:  =  G  +  V  T 

n+1  n  n 


where : 


F^  =  forecast  position  for  nth  measurement 


=  nth  measured  position 


G  =  estimated  position  after  nth  measurement 
n 


V  =  estimate  of  velocity  after  nth  measurement 
n 


T  =  time  interval  between  measurements 


a  =  position  damping  factor 


8  =  velocity  damping  factor 

b.  In  most  previous  real  time  weapons  systems,  these  equations  have  been 
applied  in  a  Cartesian  co-ordinate  system,  one  set  of  equations  being  used  for 
each  dimension.  It  may  be  seen  that,  for  positional  smoothing,  if  ex  -  0,  all 
sensor  information  is  ignored  whereas  if  ot  8  1,  there  is  no  smoothing  of 
jiositional  information.  Similarly  8  8  0  causes  sensor  information  to  be 
ignored  in  the  estimation  of  velocity  whereas  0>1  will  cause  overcorrcction 
fie  noise  amplification,  with  the  ability  to  predict  inside  turns).  Simpson 
(Reference  1)  calculated  the  limits  of  stability  in  terms  of  a  and  8,  the 
result  of  which  is  that  a  and  8  should  normal iy  bo  h.»r wecu  0  and  1. 


i 
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Tho  simplest  possible  system  is  one  in  which  a  and  3  are  fixed  constants,  and 
the  theory  of  such  systems  has  been  studied  by  Simpson.  For  such  systems, 
Bordner  and  Benedict  showed  that,  for  a  manoeuvre  expressible  as  a  ramp  input 
of  position,  the  relationship  between  a  and  B  which  optimises  noise  response 
for  a  given  manoeuvre  and  also  manoeuvre  response  for  a  given  noise  is : - 


c.  Constant  parameter  systems  suffer  from  the  incompatible  demands  that  good 
smoothing  requires  heavy  damping  (ie  small  values  of  a  and  8) ,  while  good 
response  to  manoeuvres  requires  light  damping  (ie  a  and  B  large).  Light 
damping  and  therefore  poor  noise  response,  can  lead  to  low  probabilities  of 
weapon  sensor  acquisition  and  p’ot-to-track  association  problems.  Heavy 
damping,  implying  poor  manoeuvre  response,  can  cause  sudden  loss  of  tracks 
(sometimes  termed  track  death)  through  failure  to  associate  with  subsequent 
plots. 

d.  These  limitations  led  some  workers  to  opt  for  variable  parameter  systems 
where  a  and  6  are  varied  according  to  the  state  of  the  track.  Some  systems 
have  been  developed  wherein  a  and  B  were  initially  selected  arbitrarily,  and 
changed  during  program  development  by  trial  and  error,  various  operational 
sets  of  values  being  derived  for  various  states  of  track.  Such  methods  are 
adaptive  and  are  usually  economical  in  computer  use,  both  in  terms  of  required 
storage  and  run  time,  but  generally  have  no  theoretically  optimum  adaptation. 

e.  More  recently,  processes  have  been  developed  in  which  a  and  B  are  made  to 
change  with  time  in  order  to  continually  compute  the  least  squares  line  through 
the  observations.  Such  approaches  assumed  that  errors  are  equally  distributed 
in  x  and  y  and  had  a  constant  standard  deviation.  The  formulae  for  changing 

a  and  B  in  this  manner  were  worked  out  by  Marks  (Reference  2).  For  the 
incorporation  of  the  nth  measurement: - 


_  2(2n-l) 

n  (n+1) 

(1) 

6 

n(n+l) 

(2) 

f.  It  is  clear  that,  for  large  n,  a  and  3  tend  to  0,  ie  observations  will 
be  increasingly  ignored.  This  suggests  that  there  should  be  some  maximum 
value  of  n.  To  the  best  of  the  author's  knowledge,  the  maximum  value  used 
is  generally  7  to  15.  Such  a  method  may  be  made  adaptive  if  a  means  of 
detecting  changes  in  motion  is  used,  ie  if  turn  detection  is  provided.  Then, 
if  a  turn  is  detected,  the  values  of  a  and  B  may  be  raised  simply  by  lowering 
n.  Doing  this  will  improve  the  turn  following  capability.  Another  approach 
to  turn  following  is  to  assume  the  turn  is  circular,  and  to  try  to  track  round 
this.  However,  this  requires  that  the  turn  be  quickly  detected  and  that  turns 
generally  be  of  sufficiently  long  duration. 

g.  As  has  been  previously  stated,  the  preceding  methods  are  fundamentally 
Cartesian.  There  are  also  methods  in  which  the  track  data  is  held  as  x-,  and 

co-ordinates,  course  and  speed.  Assuming  that,  for  an  aircraft  the  course 
is  more  likely  to  change  than  the  speed,  this  method  allows  speed  to  be  more 
heavily  damped  than  course. 

h.  Magowan  (Reference  3)  took  a  fresh  look  at  the  subject  and  produced  a 
logic  which  takes  account  of  the  usually  polar  nature  of  plot  noise.  This 
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automatically  allows  for  the  fact  that  the  area  of  uncertainty  round  a  plot  is 
a  function  of  range  as  well  as  the  range  and  bearing  errors.  The  method  stored 
track  data  in  range,  bearing,  course  and  speed.  However,  the  main  difficulty 
of  the  approach  is  that  the  last  best  estimate  of  position  of  a  track  is 
treated  as  having  zero  error. 

k.  More  recently  still,  Clynick  and  Milner  (Reference  4)  proposed  to 
calculate  an  optimum  time  constant  (ie,  in  effect  an  optimum  value  of  n)  based 
on  the  target  range  and  derived  velocity.  This  was  an  attempt  to  achieve  a 
reasonable  compromise  performance  in  calculating  course  and  speed  over  a 
considerable  range  of  operating  conditions. 

m.  The  approach  followed  in  this  report  is  based  on  Kalman  filter  theory, 
which  can  take  the  polar  nature  of  plot  noise  into  account  and  can  provide  the 
exponential  weighting  in  the  form  proposed  in  Reference  4.  It  is  not  proposed 
to  derive  the  Kalman  filter  equations  in  this  report,  since  this  has  been  done 
adequately  in  References  5,  6,  7  and  8.  Instead,  a  "picture"  will  be  given 
for  a  single  dimension  case. 

A  SIMPLE  APPROACH  TO  LEAST  SQUARES  SMOOTHING 

9.  Before  dealing  with  the  Kalman  filter,  we  shall  try  to  introduce  the  concepts 

involved  in  smoothing,  and  to  show  why  a  least  squares  criterion  is  used. 


a.  Let  us  assume  that  we  have  two  independent  estimates,  x'2  and  x?  of  the 
same  variable,  with  mean  square  errors  (variances)  a'  and  r  respectively. 
Assuming  that  the  errors  have  Gaussian  distributions  then  from  the  first 
estimate,  the  p.d.f.  for  the  probability  that  the  true  value  is  x  is : - 


Pi  =  v/2Ka'  exp  f  ‘  (Xia^ .  * 


Similarly,  from  the  second  estimate,  the  p.d.f.  for  the  probability  that  the 
true  value  is  x  is: 


P2 


1  f  (x?-x)2  ’ 

=  fin  exp  >  -  2F~  * 


The  joint  probability  that  the  true  value,  from  both  estimates,  is  x  is  p  =  pjp2 
where 


P  = 


exp(  .  (x'-a-xl2.  _  (£L -jQl  } 

2-n/a'r  2a'  ‘ 


2r 


Now  it  may  be  easily  shown  (see  Appendix  C)  that  the  product  of  these  two 
Gaussian  distributions  is  another  Gaussian  distribution  given  by 


_  „  t  (x-x)2  t 

p  =  constant  x  exp  \  -  - - —  } 

2S2 

iv’nere  x  =  (X2  r+X2a’)/(a'+r)  (3) 

ar.d  o2  =  a'r/(a'+r)  (4) 

This  joint  probability  is  therefore  maximised  when  x  =  x,  while  the  standard 
deviation  associated  with  x  is  0,  which  is  always  less  than  a*  and  /r.  The 
actual  value  of  the  constant  in  the  expression  for  p  does  not  matter  here, 
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since  we  do  not  want  +<  know  t-,.e  actual  value  of  p,  but  rather  th  value  of  x 
which  maximises  it.  (.The  "constant"  in  the  expression  is  not  strictly  a 
constant,  since  it  is  a  function  of  the  a*  and  r,  x'2  and  X2,  but  the  important 
point  is  that  ii  is  not  a  functi 'n  of  x  which  is  the  quantity  of  interest.) 

b.  It  is  -.c  doubt  apparent  that  x’2  coul  ’  have  represented  a  forecast  of  x, 
and  that  x?  ould  have  represented  a  measurement.  If  x  changes  with  time,  it 
is  also  ner  _ary  o  e.c  v.imate  the  rate  of  change  (which  we  will  assume  to  be 
constant) . 


I, 


c.  Let  us  ide-;  tb:  .  '  ->t  x'2  i?  an  estimate  baseu  on  a  previous 

(estimated)  p  siticn  x^  ■.  i;  there  is  an  -stimated  velocity  Vj,  and  that  X2 
is  an  observed  'n,  Le*  ^sume  that  x1 1  has  variance  a1  (as  before)  and 
that  X2  has  variance  r.  w;  also  assume  that  xi  has  variance  a,  that  vj  has 
variance  d^  and  that  the  covariance  between  xi  and  vj  is  b.  (The  covariance 
of  xi  and  vi  is  defined  thus:  if  an  error  in  xj  is  6xi  and  in  vj  is  6vj  then 
the  covariance  of  and  vi  is  the  mean  value  of  all  products  (6xj  ov i).  This 
is  analogous  to  the  variance  of  a  single  variable  being  the  mean  square  error: 
the  covariance  is  the  mrn  value  of  the  product  of  errors  in  two  variables.) 


Appendix  C  shows  that  the  best  estimate  of  velocity  after  making  the 
measurement  X2  may  be  written :- 


v  = 


where  vj 
b 
a 
t 
d 


v,  (b+td) (xo-x'2) 

1  i.-2bt+t2d+r 

=  previous  estimate  of  velocity 

=  covariance  of  previous  position  and  velocity  estimates 
=  variance  of  previous  position  estimate 
-  measurement  time  interval 
=  variance  of  previous  velocity  estimate 


and  that  its  variance  is  given  by  s2  where: 

_2  =  d(a+r) -b2 

a+2bt+t:id+r 


We  may  reconfigure  (3)  to  have  the  same  form  as  (5)  hence:- 


X2  .  X!  ♦ 


or,  in  terms  of  the  a,  b,  d  etc, 
x2  =  x'2 

Hence  we  may  write: 


,  (a+2bt+t2d)  ( xp  - x 1 2 ) 

a+2bt+t^d+r 


x2  =  x'2+a(X2-x'2) 


V2  =  V !+  |  (x2-x'2) 
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(5) 


(6) 


(7) 


(8) 


(9) 

(10) 
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where  a 


a1 

a'4-” 


a+2tb+t2d 

a+2tb+tzd+r 


and 


_ t  (b+td) 

a+2tb+t2d+r 


Now,  o2  is  the  new  value  of  a,  and  s2  is  the  new  value  of  d.  It  can  readily  be 
shown  that  the  new  value  of  b  is 


(b+td)r 

a+2Ft+Pd+r  ’ 


d.  Initial  Conditions 


Let  us  consider  two  initial  measurements  xq  and  Xj  taken  time  t  apart.  The 
best  estimate  of  position  is  thus  xj,  and  the  estimate  of  velocity  is 

xi-x0 

t 

If  there  is  an  error  in  measurement  of  xq  of  5xo  and  one  in  xi  of  Sx^,  then 
the  error  in  estimating  velocity  is  (Sxj  -  <5xo)/t. 

The  variance  of  this  velocity  estimate  is  the  mean  value  of 


(5xj  -  5x0)2  _  (6xi)2  -  26x^6xi  +  (6xp)2 

t»  t 

The  mean  value  of  6xn6x!  =  0  for  uncorrelated  measurements,  hence  variance  of 
Vi  =  d  =  (mean  (6xi)2  +  mean  (6xo)2)/t2 

=  Ui  +  r0)/t2,  say. 

The  covariance  of  xi  and  Vj  (called  b)  is  the  mean  value  of: 

5x:(<5xi  -  oxo) 
t 

which  gives  b  =  . 


Thus  we  may  construct  a  covariance  matrix  which  relates  to  xj  and  vi  and  is: 

r  ,*i 
a  b 

b  d 


whose  elements  are  the  mean  values  of  the  elements  of 


I  1^x3  [*.X!  5v!  ] 


\  H 


where 


-  rlf  b  =  II  and  d  =  £iT£o 
c  t 
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We  would  have  obtained  the  same  result  by  applying  Appendix  B  equation  3  to: 


!r0  O' 

'  ,  using  the  transformation 

)0  ni 


.u  rn 

-J 


r*»i 

”o 

1  ;  x0 

1  i 

|  J 

=  i_ 

i  ! 

Lvi! 

“t 

t  LXL 

We  wish  to  forecast  over  a  time  interval  t  to  calculate  a  forecast  position 

x'2. 

We  thus  use  the  transformation 


It  Xj 

0  1  vj 


We  may  apply  Appendix  B  equation  3  to  calculate  the  covariance  matrix  for  the 
forecast  state 


This  is 


:a+2bt+t2d  b+td 
! 

b+td  d 

If  the  variances  of  measurement  error  are  assumed  equal  and  since 

a  =  ri,  b  =  p-  ,  d  =  and  writing  a2  =  rj  =  tq 

we  find  that  the  covariance  matrix  for 

~  1 


iso* 


is:  '  ‘ 

v-.  .  io2  2o2  ; 

L~  t2  j 

When  we  combine  the  forecast  x'2  with  the  observation  x2 ,  which  has  variance  r 
using  (5)  and  (8)  we  get 


v  =  v,  - 

1  t(5o*+a2) 

■  b  * 

h  -  *V 

=  v.-+  $1*22*12) 

*  2  6 


m 


.v,WW<7.w .?  tt.^t: 


7"  i-^tyyn^v  -’ 


'7"’^9T 


1'luts,  t'or  constant  variances  of  observation  of  errors,  we  find,  using  the 
definitions  of  the  two  damping  factors  given  by  (9)  and  (10),  that  a  =  5/6 
and  i<  =  1/2  arc  the  optimum  values  for  incorporation  of  the  third  plot  into 
the  track. 


Marks  (reference  2),  has  shorn  that,  for  constant  errors  and  time  intervals: - 


a 


2(2n-l) 

n(n+l) 


n(n+l) 

where  n  is  the  observation  number,  the  first  observation  for  the  track  having 
n  =  1. 

In  the  above  example  n  =  3,  which  gives  a  =  5/6  and  6  =  1/2. 

Nov:  in  (9),  a  is  effectively  defined  as  the  ratio: 

variance  of  forecast  position _ 

variance  of  forecast  position  +  variance  of  observation 

If  a  is  less  than  0.5  it  therefore  means  that  the  variance  of  forecast  position 
is  less  than  the  variance  of  the  observed  position.  When  n  =  6,  a  =  11/21; 

•‘hen  n  =  7,  a  *  i3/28.  Thus,  when  n  =  7  the  forecast  is  about  as  accurate  as 
the  measurements.  Also,  the  variance  of  the  smoothed  position  =  ar,  which,  for 
n  =  7  gives  13r/28.  Thus  when  n  =  7  the  variance  of  the  smoothed  position  is 
iust  less  than  half  the  measurement  variance,  ie  the  "noise  power"  has  been 
halved. 

It  is  not  proposed  to  go  any  further  with  this  example,  since  its  purpose  was 
solely  to  introduce  the  basic  concepts  of  tracking,  but  the  illustration  serves 
to  demonstrate  the  following  drawbacks  of  this  simple  approach. 

c.  The  example  was  given  for  tracking  in  one  dimension  only,  and  assumed  that 
the  time  interval  between  observations  was  constant.  If  we  wish  to  track  in 
more  than  one  dimension,  this  simple  theory  can  only  work  if  the  measurement 
errors  in  all  the  dimensions  are  not  related  in  any  way  (ie  are  uncorrelated), 
in  order  that  the  tracking  may  be  separately  performed  in  those  dimensions. 

Now,  radars  with  auto-extraction  equipment  have  independent  errors  in  range 
and  hearing.  This  would  constrain  tracking  to  tracking  in  range  and  bearing: 
linear  forecasting  is  then  not  good  enough  for  finite  range  and  bearing  rates. 
I'hc  theory  is  only  valid  for  tracking  in  x  and  y  when  the  variances  in  x  and  y 
can  be  considered  independent  and  constant.  This  is  so  in  manual  detection 
systems  which  employ  Cartesian  frames  of  reference. 

It  should  be  remembered  that,  if  variances  in  range  and  bearing  are  constant, 
t.ie  variances  of  x  and  y  after  transformation  from  the  r66  to  the  xSy 
,o-orJinato  system  will  be  related  and  will  be  functions  of  r  and  6  (or  x 
and  }■}  and  will  not,  therefore,  be  constant  for  a  moving  target. 

Tne  Kalman  filter,  which  forms  the  basis  of  this  tracking  study,  does  not 
suffer  from  the  restrictions  above:  it  is  a  generalised  version  of  the  method 
empirically  derived  and  is  in  matrix  form  -  thereby  conveniently  handling  the 
correlations  between  errors.  The  Kajiman  filter  equations  were  first  derived 
by  Kalman  and  Bucy,  (References  6,  7) ,  but  many  others  have  produced  different 
derivations  from  different  viewpoints  (for  example  see  References  5  and  8). 
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10.  KALMAN  FILTERING 

a.  In  this  section,  all  upper  case  letters  will  refer  to  matrices  and 
lower  case  letters  to  scalars.  We  will  take  the  ideas  already  presented 
and  represent  them  in  a  different  way  as  an  introduction  to  the  Kalman 
filter  equations. 


Equation  (11)  states 


I*'*  _ 

i  t 

1 

- 1 

*—< 

0  1 

.  xll 


LVlJ 


which  means  that  the  state  of  the  track  which  is  given  by  xi  and  vi  is 
transformed  to  another  state  given  by  x'2»  vi  over  a  time  interval  t.  We 
may  represent 


*1 

Vi 


as  Xi  and  hence  represent 

fl  tl 


i 

LV1J 


as  X*2  ■ 


The  matrix 


!o  ii 


which  we  may  call  4>  transforms  Xi  to  X'2 


We  may  thus  write  equation  (11)  as  X'2  =  OXj  or,  in  terms  of  transition  from 
state  X^  to  state  X', 


k-i  1 , 


X',  .  =  $X.. 
k+1  k 


(13) 


This  expression  is  in  effect  the  equation  of  motion  from  which  a  future  state 
X'k+j  can  be  calculated  from  a  past  state  X^. 

The  covariance  matrix  associated  with  Xi  (or  X. )  was  given  in  (5)  as 


!a  b 


Pi  = 


i  -  |  ^  ~  ‘k' 

b  d 


(  -  Pk). 

That  associated  with  X'2  (or  X'^j)  was  found  in  Appendix  C  as 
iat-2bt+t2d  b+tdl 


2  = 


i  b+td 


d  ! 


<  -pw* 


In  fact  the  covariance  matrices  are  related  by  the  equation: 


P',  .  =  4>  P.  4>  , 
k+1  k 

T  . 


the  superscript  indicating  the  matrix  transpose.  This  may  be  checked  by 


deriving  ^  ^T  and  comparing  with  P'2* 
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a  was  defined  bv  (9)  as:  ~-^tt~t^-t~~  .  r  being  the  measurement  variance 
•  v  a+2bt+t‘d+r  *  ** 


and  3  was  defined  by  (10)  as: 


(b+td)t 


a+2bt+t2d+r 


The  denominator  of  both  of  these  quantities  is  the  (1,1)  element  of  P'2  plus  r. 
We  may  obtain  this  (1,1)  element  of  P * 2  by  the  operation 


[1  0]  P'2 


=  a+2bt+t2d. 


Hence,  if  we  define  a  matrix  M  =  [10],  the  denominator  for  a  and  3  is 


(MPM  +R) ,  where  R  is  the  measurement  covariance  matrix  (which  is  the  same  size 
as  the  MPMt  product  and  is  a  1X1  matrix  in  our  example). 


The  numerators  for  a  and  0/t  are  the  contents  of  the  first  column  of  P'^+j  in 
this  example,  and  we  may  obtain  these  thus: 


T 

P*.  .  M1  = 
k+1 


ia+2bt+t2d 
b+td 


Thus  P'k+JMT  (MPMt+R)_1  gives  a  matrix  which  we  shall  call  K. 


k+1 


g 

where  the  first  element  of  is  a  and  the  second  is  p 


Our  smoothing  equations  are: 
x 

K+i 

3 


.  .  +  ot(x.  ,  -  x',  , ) 
k+1  v  k+1  k+1' 


vk+l  =  vk  "  t  (xk+l  *  k+lJ 
which  is  clearly  the  same  as  writing: 


,) 


Xk+1  =  X'k+1  +  Kk+i  (Yk+l 


MX’k+l> 


where  Yj.  +  ^  is  the  matrix  of  observations,  and  is  1X1  for  this  case. 


It  is  more  usual  to  write  this  as: 


Vi  ■  x'w  -  Kk.i  (M,c'k.i  -  W‘ 


Ihe  variance  of  x^+1  was  found  in  (4)  to  be:  Q2  =  . 


In  (12),  a',  the  variance  of  forecast  position,  was  found  to  be  given  by: 
a'  =  a+2bt«-t2d. 


Hence  we  may  rewrite  (4): 

-2  -  r(a+2bt+t--d) 
a+2bt+t2d+r 


■10- 


,  **21. xs;^. ..  t  •  • 


-  -  at  ^  •  ■  -  v-  BS^M6|aaaaMiia  - 


We  may  further  rewrite  this  as  follows: 

52  ■  ■*»***  - ! 

Now,  =  [a+2bt+t2d  b+td] 

■”«  <»•»)  of  *  (K'2bttf2dIr 

Thus  it  is  correct,  for  the  (1,1)  term  at  least,  to  write: 
k+1  k+1  k+1  k+1 

We  may  now  move  on  to  a  more  formal  definition  of  the  Kalman  filter. 

b.  The  Kalman  filter  equations  which  are  derived  in  References  6-8  may  be 
written  thus: 

X'k*l  =  *k  Xk  04) 

p'k.i  ■  \  pk  *1  ♦  \  \  i  '15> 

Kk.i  ■  p’k.i  “Tk.i  |V  p,M  MI.1  *  Rk.i] ''  (16> 

xk.l  ■  x’k.l  -  "k.l  <"k.l  x'k.l  -  Yk.l>  (17> 

pk.i  "  p,k.i  -  xk.i  Vi  p'k.i  (18) 

where  a  prime  (')  represents  a  forecast  value,  a  hat  (')  represents  a  best 
estimate  of  a  variable,  superscript  T  is  the  transpose  of  a  matrix  and: 

X  is  the  state  vector  of  order  nxl 

i>  is  the  transition  matrix,  nxn,  and  defines  the  transition  from  true 
state  to  true  state  Xj,+^ 

P  is  the  nxn  covariance  matrix  of  the  estimate  of  X  (ie  P'j(+^  is  the 
covariance  matrix  for  estimate  X'^j) 

G  is  nxj  matrix  representing  the  effects  of  j  elements  of  ’plant  noise' 
sometimes  known  as  process  noise  on  the  n  elements  of  the  state  vector; 

Q  is  a  jxj  matrix,  being  the  covariance  matrix  of  the  plant  noise 

Y  is  an  raxl  vector  and  has  as  its  elements  the  measured  variables 

M  is  the  (so-called)  measurement  matrix  and  is  mxn 


R  is  the  mxm  covariance  matrix  of  the  measurements  Y 


K  is  a  matrix,  nxra  and  essentially  contains  the  damping  factors. 
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c.  Some  explanation  of  these  terms 


The  true  state  at  time  tk  is  X^ 


The  true  state  at  time  tk+i  is 


Vl 


\\  *  GkUk 


where  is  a  set  of  random  inputs  known  as  plant  noise.  In  essence,  the  G,  U, 
Q  terms  represent  Low  true  the  equations  of  motion  implied  in  $  are,  or,  for 
example,  how  straight  a  nominally  straight  track  really  is.  This  noise  is 
assumed  to  have  zero  mean,  thus  the  best  forecast  is  as  given  in  13.  Having 
thus  mentioned  plant  noise,  we  will  now  ignore  it  for  the  purposes  of  the  rest 
of  tie  report,  since  the  main  effect  is  analogous  to  the  time  constant  concept 
in  Reference  4  which  is  not  of  concern  at  present,  but  will  be  discussed  in  a 
future  report.  (It  may  be  apparent  to  the  reader  that  the  effects  of  plant 
noise  reduce  the  rates  at  which  variances  of  successive  estimates  change.) 
Other  terms  may  be  used  in  Kalman  filters,  these  are  also  ignored  for  the 
purposes  of  the  report. 

d.  Incorporating  a  measurement 

When  a  measurement  is  made,  what  is  actually  observed  is  some  function  of  the 
true  state  together  with  noise. 

Thus  what  is  observed  is: 


Yk+1  -  Mk*l  Xk+1  +  Nk+1 


(19) 


where  Xk+1  is  true  state  at  t  *  tk+1,  and  nk+3 
This  equation  then  defines  Mk+^. 


is  a  set  of  noise  components. 


e.  Working  through  the  Kalman  Equations 


As  a  simple  example  consider  a  state  vector  to  represent  a  position  (say  x) 
and  a  velocity  (say  x) .  Let  us  consider  that  x  is  observable  (as  Y  =  x+n)  but 
x  is  not.  Then: 


\  represents 


x 

x 


and  Y  represents  a  measurement  of  x,  thus  Y  will  be  a 


lxl  matrix  which  is  the  true  value  of  x  corrupted  by  noise.  In  this  example 
M  is  obviously  a  1x2  matrix: 


[1  0] 

Hence  Y  =  [1  0] 


xi 

X 


+  [N] 


f .  Forecasting 


In  this  simple  example  we  assume  linear  equations  of  motion,  hence: 


x'  =  x  ♦  x(At) 


(At  being  tk+1-tk) 


x  =  x 
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or,  in  matrix  form 


-  •* 

- 

X' 

1  At 

X 

X' 
_  «. 

?  K 

_* 

Comparison  with 


(14)  shows 


that  we  have  defined  $  to  be: 


1  At 
0  1 


To  simplify  matters  slightly,  we  will  write  t  instead  of  At.  This  new  variable 

should  strictly  be  written  t,  ,  .  but  is  written  as  t  for  obvious  reasons. 

K+  1 


Let  us  assume  that, 


a 

b 


i 


at.  t  =  tk,  Pk  is: 


(this  matrix  is  symmetrical  since  the  covariance  of  x  and  x  equals  the 
covariance  of  x.  and  x) . 


Hence  P'k+1  = 

i- 

1  t, 

i 


a 


>0 


l! 


b 


b  ' 


1 

t 


0 
1  ■ 


|a+2bt+t2d  b+td  | 

!  ! 

ib+td  d : 

L 


Now  K  is  nxm  and,  as  stated  earlier  has  one  element  which  we  will  call 


>+r 


K 


k+1 


-  tjAiSSsiL  att 


'T’ 

a+2bt+b2d 

b+td 

m 

M.  ,  P*  .  M  .  .  =  [1  0] 

k+1  k+1  k+1  1  J 

b+td 

d. 

1° 

=  a+2bt+t2d. 

• 

•  a+2bt+t2d 

b+td*  fll 

_  I 


1  b+td 

i 


L°. 


[a+2bt+tzd+rk+1] 


a+2tb+t2d 


a+2tb+t^d+r, 


k+1 


b+td 


a+2tb+t2d+r. 


k+lj 
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Now,  the  expression  a+t(2b+td)  is  the  variance  of  the  forecast  position  x'k+^. 

If  we  call  this  a'.  ,  then  the  first  element  of  K,  ,  is 
k+1  k+1 

a',  , 
k+1 

a',  ,+r,  . 
k+1  k+1 

If  this  is  compared  with  (9)  it  will  oe  seen  that  this  is  precisely  a.  It  will 

also  be  seen  that  the  estimate  of  velocity  variance  in  (6)  was  in  fact  a 

combination  of  velocity  variance  and  position  velocity  covariance.  It  will  then 

be  clear  that  the  second  element  of  K,  ,  is  the  fL  defined. 

k+1  t 

g.  Smoothing 

We  can  now  move  on  to  (17). 

Mk+i  rk+i  is  simPly  x'k+r  'mus> 


(a+2bt+t2d)(x»k+1  -  yk+1) 
a+2bt+t2d+rk+1 
(b.td)(x'k^  -  yktl) 

-  a+2bt+t2d+r,  . , 


Similarly,  using  (18), 


•a+2bt+t2d  b+td  ! 


a+2bt+t2d 


P  =  * 

k+1  i 


'b+td 

! 

L 


I  a+2bt+t2d+r, 

i _ b+td 

|  a+2bt->  t2d+r. 


[1  0]  a+2bt+t2d  b+td 
b+td  d 


k+1  J 


To  simplify  the  manipulation,  we  write 
a'  =  a+t(2b  +td),  and  r  =  rk+^ 

th  en 


x'k+ll  la'+r  (x'k+l  '  yk+P 

*.  }  '  ! 
k*ll  | (b+td)  ,  , 

\  i.  a'+r  1  k+1  yk+l). 


fa*  b+tdj  "^—1  [10]  fa'  b+t 

!b+td  dj  [b+td 


a'  b+td 
b+td  d 


(a')2 

a'+r 

a* (b+td) 
a'+r 


a* (h+td) 
a'+r 

(b+td)2 

a'+r 


a'r  (b+td) r 

a'+r  a'+r 


(b+t.d)r  d(a+r)  -b2 
a'+r  a'+r 


The  variance  of  estimated  position  at  (k+1)  may  be  seen  to  be  exactly  that 
derived  earlier  in  the  single  dimension  case.  Similarly,  as  mentioned 
earlier,  the  first  element  of  Kj.  corresponds  exactly  with  the  derived  a 

and  the  second  element  with  £ 

t- 


h.  Initiation  of  the  filter 


The  next  problem  to  consider  is  that  of  starting  up  the  filter.  However,  the 
starting  of  the  filter  requires  knowledge  which  may  not  be  available.  In 
particular  when  we  have  received  one  observation,  we  need  a  velocity  estimate 
and  its  variance.  Let  us  consider  that  we  have  a  stream  of  measurements  y^ 
each  of  variance  r.  Thus, 


assuming  zero  velocity. 


Thus  X'i  = 


at  time  At  . 


We  know  that  the  variance  of  yQ  was  r.  We  assumed  that  x  was  zero,  thus  we 
are  justified  in  assigning  a  large  variance  to  this  value.  In  reality,  we 
may  assume  initial  velocity  components  such  that  the  target  is  approaching. 
It  should  also  be  possible  to  use  a  sensible  value  for  the  variance  of  this 
estimate  since  target  speeds  ai'e  limited. 


The  covariance  between  x0  and  x0  is  zero  since  the  errors  ‘n  them  are  net. 
related. 


Hence  P0  = 


Thus  P[  = 


Kj  =  P 


,  L  being  a  large  number. 

tL 

Ll 

"1 


'[i  o]p; 

T 

+r 

0 

—  - 
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.-.  X,  = 


2r+t2L  i;o'1J- 
tL 

2r+t2L 


Since  L  is  large  (at  present,  at  any  rate) 


y0-y0+yi 

yi-y0 

*-  t 


which  is  perfectly  sensible. 


yi-y0 


M1  -  KiMjP'i 


=  P’l  -  ! 


r 

I  r+t2L 
1 2r+t^L 


[1  0] 


r+t2L  tL 


|  tL 
i2r-*-t2L 


I  (r+t2L)  (2r+t2L-r-t2L)  tL(2r+t2L-r-t2L) 


2r+t2L 

'tL(2r+t2L-c2-t2L) 
j  '  2r+t2L 

r(r+t2L)  rtL 
2r+t2L  2r+t2L 


2r+t2L 

L(2r+t2L-t2L) 

2r+t2L 


1 2r+t2L  2r+t2L 


Now,  since  L  is  large, 


ihe  '•  irianoe  of  the  position  .-q  is  thus  r,  and  that  of  estimating  jq  i- 

both  of  which  are  reasonable  results,  and  in  keeping  with  those  derivec 
empirically  in  (4). 


7J7r;sj/^»e* v*« 'r*» V«*- ■ *’  **~ 


We  can  easily  show  that  the  covariance  terms,  £  tie  in  with  the  empirical 
theory  by  calculating  P'2  and  showing  that  ther  variance  of  x'2  is  5a2. 

p»2  =  ♦Pi*7 


■1  tl 

rl 

r  t 

1  0 

0  l| 

r  2r 

t  1 

l  -1 

'-t  t^J 

L  J 

*1 


3r  2r  i 

t2J  • 

The  filter  may  also  be  started  by  observing  the  first  two  plots  and  calculating 
initial  velocity,  varianc  ■js  etc. 

j .  Equivalence  of  Kalman  and  ct-g 

Thus  the  Kalman  filter  has  been  shown  to  be  equivalent  to  the  least  squares 
ag  tracker  provided  that: 

(1)  all  measurements  have  equal  variance 

(2)  data  rate  is  constant 

(3)  there  is  no  plant  noise  (ie  the  track  really  does  follow  the 
assumed  equations  of  motion) . 

(4)  an  error  in  one  co-ordinate  of  the  track's  position  does  not  affect 
the  other  co-ordinate. 

Hence  the  least  squares  ag  tracker  is  a  restricted  Kalman  filter. 

Restrictions  (1)  and  (2)  become  important  when  information  is  derived  from 
two  or  more  unsynchronised  sensors  of  different  accuracies. 

Restriction  (3)  is  difficult  to  deal  with  in  the  ag  tracker.  Item  (4)  is 
perhaps  the  most  significant  restriction.  If  a  track  is  stored  as  range, 
range  rate,  bearing  and  bearing  rate,  then  the  errors  between  range  and 
bearing  will  not  be  related  (unless  data  is  input  manually  from  a  device 
driven  in  Cartesian  manner).  If  the  track  is  represented  in  Cartesian 
co-ordinates  and  their  rates,  the  errors  are  related  unless  data  is  input 
from  a  Cartesian  device. 

It  is  shown  in  Appendix  A  that  the  Kalman  filter,  the  least  squares  ag  tracker 
and  linear  regression  give  the  same  results,  for  special  conditions  mentioned 
above.  When  plant  noise  is  absent,  the  Kalman  filter  represents 
multi-dimensional  linear  regression  worked  out  on  a  continuous  basis. 

k.  Choice  of  equation  of  motion 

As  has  been  mentioned  previously,  radar  information  is  available  as  a  range 
and  bearing,  with  the  errors  in  the  two  measurements  unrelated.  At  first 
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sight,  it  may  appear  reasonable  to  attempt  to  store  tracks  as  range,  range 
rate,  bearing  and  bearing  rate,  ie  to  use: 


1  t  o  o 


o  1  o  o 


o  o  1  t 


o  o  o  1 


Such  a  representation  attempts  to  fit  a  spiral  through  all  plots  which  is  not 
generally  valid  particularly  for  targets  which  pass  by  the  origin.  However, 
this  error  is  not  significant  for  crossing  at  zero  or  large  ranges.  Crossing 
at  zero  range,  is  of  course,  radial.  If  a  system  is  only  interested  in  point 
defence,  such  a  method  may  be  adequate.  The  effect  may  be  reduced  by  use  of 
the  time  constant  giving  exponential  decay  referred  to  earlier.  In  the 
simple  a3  system  this  would  be  derived  (approximately)  by  fixing  a  maximum 
value  of  n.  In  the  Kalman  filter,  introduction  of  plant  noise  terms  has  a 
similar  effect.  Such  a  term  must  be  used  anyway,  otherwise  the  filter  will 
eventually  ignore  plots  since  the  variances  or  plots  would  become  very  large 
compared  with  forecast  variances. 


One  would  expect  that  use  of  a  second  order  state  vector  would  improve  matters 
for  straight  tracks.  However,  one  would  expect  that  it  would  still  tend  to 
pull  tracks  into  the  origin,  exhibit  less  damping  than  the  first  order  r6 
tracker  and  have  better  response  to  manoeuvres. 

for  such  a  system  the  equation  X'^+1  =  would  expand  into:- 


K.i  ! 


i  r ' 

:  k+1 


U. 

i  k+ 1 


1  t  — r- 


o  1  t 


O  0  0 


O  0  0 


i  o  o  1  ooo 


0  0  o  1 


,  ;  0  0  0  0  1 

K  *  1  !  , 


'  k+i 


‘  0  0  0  0  0  1 


However,  this  is  still  not  a  perfect  representation  of  a  straight  track  and  it 
has  the  demerits  of  complication  and  the  large  variances  associated  with 
estimates  of  r  and  6k.  We  therefore  need  to  transform  measurements  from  r  and 
c>  to  x  and  y.  This  is  simple  enough: 
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x  =  r  sin  0 


y  =  r  cos  0  (0  being  interpreted  in  the  nautical  sense). 

The  covariance  matrix  of  r  and  0  is 


It  is  shown  in  Appendix  D  that  the  covariance  matrix  for  x,  y  is 
o2rrSin20+r2a2eQCosia  (o2rr-r2o200)SineCos0 

Rk(x,y)= 

(a2  -r2a2  )Sin0Cos8  a2  Cos20+r2a2  Sin20 

rr  yy  rr  yy 


provided  that  o0Q  =  sino00,  which  is  true  for  all  practical  radars. 

Hence,  all  measurements  are  transformed  to  Cartesians,  the  R  matrix  being 
transformed  as  above.  The  system  is  then: 


11.  TESTING  RESULTS 

a.  Each  of  these  three  types  has  been  programmed  on  a  computer  for 
comparative  testing.  A  simple  simulation  has  been  built  round  each  of  the 
trackers:  the  simulation  generates  tracks  which  consist  of  two  straight 
parts,  the  parameters  for  these  parts  being  provided  as  input  data.  Noise 
is  superimposed  on  each  generated  plot,  the  noise  being  added  as  range  and 
bearing  errors.  The  noise  is  produced  by  a  noise  generator  which  produces 
Gaussian  distributed  pseudo-random  noise  (See  Appendices  E,  F  and  G) . 

The  trackers  were  tested  with  various  sets  of  input  data.  The  variances  of 
errors  in  range  and  bearing,  and  the  data  rate  were  all  kept  constant  for  any 
one  run.  This  was  not  due  to  any  limitations  of  the  trackers,  but  purely  in 
order  that  the  whole  system  could  be  set  to  work  quickly.  Also,  keeping 
things  simple  in  this  manner  made  it  easier  to  predict  what  ought  to  happen. 

Some  tracks  with  turns  included  were  generated:  it  was  not  expected  that 
performance  should  be  good  on  turns,  since  no  turn  detection  or  turn  handling 
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facilities  have  been  built  in.  Overshoot  on  turns  was  shown  by  running  some 
of  the  tracks  without  noise  (the  R  matrices  were  not  zeroed  as  the  filter 
would  ignore  measurements  after  the  first). 

b.  The  results  obtained  were  very  much  as  expected,  as  may  be  seen  from  Figs. 
1-15.  Both  types  of  polar  tracker  perform  tolerably  well  with  tracks  which 
turn  towards  the  origin,  but  poor  performance  against  crossing  targets.  If 
the  distance  units  are  considered  as  Km  and  time  units  seconds,  1  distance 
unit  per  time  unit  corresponds  to  approx  Mach  3.  One  would  expect  the  r0 
trackers  to  have  poor  performance  for  targets  which  turn  away.  Thus  the 
polar  trackers  tend  to  assume  all  tracks  are  going  to  approach  the  origin, 
whereas  the  Cartesian  trackers  do  not. 

The  standard  deviations  of  the  plot  noise  were  0.4  distance  units  ror  range 
measurements,  and  0.4°  in  bearing. 

Figure  1  shows  the  response  of  the  three  trackers  to  a  noise  free  track  which 
consists  of  two  straight  parts.  All  three  trackers  are  satisfactory  on  the- 
initial  radial  portion  -  this  is  as  it  should  be.  The  responses  to  the  turn 
may  be  clearly  seen.  If  the  track  had  continued  beyond  the  point  at  which  it 
stepped,  the  xy  tracker  would  have  eventually  rejoined  it.  The  purpose  of 
tnis  graph  is  to  show  the  effect  of  continually  increasing  damping  in  the 
event  of  actual  motion  being  significantly  different  from  assumed  motion. 

How  to  deal  with  this  situation  (turn  handling)  will  be  dealt  with  in  a  later 
report . 

Figures  2-4  show  the  performances  of  the  trackers  against  a  short  noisy  track 
which  crosses  at  10  distance  units.  It  is  clearly  seen  that  the  first  order 
polar  tracker  pulls  the  track  towards  the  origin  -  because  it  is  trying  to 
fit  the  best  spiral  through  all  of  the  points.  This  effect  is  less  noticeable 
with  the  second  order  polar  tracker  (Figure  3),  and  absent  with  the  first 
order  Cartesian  Tracker  (Figure  4). 

Figures  5-15  all  have  the  same  speed,  1  distance  unit/time  unit.  Figures  5,  6, 

7  and  8  show  the  relative  performances  against  a  straight  track  with  a  crossing 

distance  of  30  distance  units.  The  first  order  r0  tracker  (Figure  6  and  7)  is 
clearly  poor  'n  that  the  incorrect  equation  of  motion  introduces  systematic 
errors.  The  2nd  order  r6  tracker  is  "reasonable"  in  removing  these  systematic 

errors  until  the  plot  at  y  =  26.3,  ie  to  plot  number  19  (see  Figure  8). 

However,  the  xy  tracker  (Figure  5)  is  considerably  better,  both  in  terms  of 
position  and  velocity:  .after  plot  6,  the  xy  tracker  always  has  velocity 
estimates  within  t  0.02  of  the  true  values.  When  the  2nd  order  polar  tracker 
applies  enough  damping  to  do  this,  the  non-linear  assumed  equations  of  motion 
start  to  "pull"  the  track  systematically  to  one  side. 

Figures  9-15  depict  a  similar  track  to  that  in  5-8,  except  that  the  crossing 
distance  has  been  reduced  to  15  distance  units.  The  performance  of  the  xy 
tracker  (Figure  9)  is  quite  reasonable,  particularly  when  Figures  10  and  11 
arc  considered.  These  two  graphs  show  plotted  and  smoothed  errors  together 
i  ith  their  c's  and  3c' s.  The  curves  of  o  and  3a  for  the  smoothed  errors  are 
approximate,  in  some  later  runs  the  true  values  will  be  output  during  the  runs. 
The  first  order  polar  tracker  does  not  perform  adequately  for  very  long 
F.gures  12-13).  The  second  order  polar  tracker  performs  adequately  up  to 
urjund  plot  11. 

It  may  be  apparent  that  limiting  the  number  of  plots  used  in  the  two  r0  based 
trackers  would  have  improved  performance.  In  particular,  if  the  track  depicted 
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in  Figure  11  had  a  time  constant  of  11  plots  it  would  probably  have  maintained 
goad  contact.  This  concept  will  be  pursued  further  in  a  later  report.  It 
is  a; so  worth  mentioning  that,  at  the  closest  point  of  approach,  the  equations 
of  motion  assumed  in  polar  trackers  are  valid  since  motion  is  circumferential 
at  this  point.  Of  course  the  above  section  demonstrates  the  importance  of 
selecting  the  correct  equations  of  motion. 

12.  USE  OF  THE  METHOD  USING  DEFERENT  MEASURED  QUANTITIES 


a.  We  may  now  make  a  further  point  about  the  usefulness  of  the  Kalman 
filter  in  application  such  as  "Bearing  only".  Consider  that  we  have,  in 
addition  to  normal  radars,  a  sensor  which  only  provides,  say,  bearing 
information.  In  a  polar  tracker,  we  may  use  this  information  readily.  It 
may  be  recalled  that  M  was  defined  in  (19). 

For  a  polar  tracker,  the  state  vector,  X,  may  be: 


j  the  measured  variables  may  be: 


N 

Y  =  !  ;with  covariance  R 

9  i 

yJ 


Hence  Mr0  may  be  seen  to  be: 


o4  o 
r 


o  o4 


1  o  o  o 


o  o  1  o 


If  we  only  measure  bearing,  (19)  indicates  that  MQ  would  be: 


[o  o  1  o] 


to  give 


[o  o  1  o]  [r 


~  0  -  noise  . 


The  covariance  matrix  applicable  to  9  ,  Rg  is  o2Q;  a  1x1  matrix. 

b.  It  may  now  appear  difficult  to  incorporate  this  single  variable  measure¬ 
ment  in  a  track  represented  in  Cartesians.  However,  the  fact  that  we  were 


able  to  make  a  bearing  meas>!rement  at  all  conveys  the  information  that  the  range 
of  the  target  was  somewher'  -etween  0  and  the  maximum  range  of  the  sensor. 

Thus  it  would  not  be  unreal  enable  to  assign  i  value  of  range  to  the  bearing 
measurement,  this  range  being,  say,  one  half  maximum  range  of  some  function  of 
signal  strength.  The  R  matrix  associated  with  the  "measurement”  would  contain 
the  bearing  variance  and  a  variance  for  range.  If  the  range  were  better  than 
just  a  guess,  then  o2r  could  be  made  reasonable.  This  would  net  prejudice  the 
assumption  made  in  Appendix  B,  since  the  approximation  for  this  transformation 
is  only  effective  in  bearing. 

13.  CONCLUSIONS 

a.  The  Kalman  filter  approach  provides  the  basis  for  an  effective  and  unified 
generalised  tracking  system.  Cartesian  representation  of  tracks  appea  -s 
advantageous  at  this  stage.  However,  when  the  effects  of  plant  noise  are 
considered,  it  would  appear  that  this  is  best  applied  in  a  track-oriented 
manner.  Alternatively  the  knowledge  of  foiecast  accuracy  could  be  used  in 
deciding  whether  or  not  a  turn  had  occurred  -  in  which  case  the  filter  could 

be  reset  in  some  fashion. 

b.  It  has  been  shorn  that  a  system  which  takes  information  from  various 
sensors  with  differing  accuracies  can  be  produced  and  that  not  all  sensors 
need  provide  the  same  sort  of  information. 

c.  It  is  perhaps  worth  noting  that  the  tracking  process  can  only  be  as 
good  as  the  data  it  receives.  Tius  it  is  dependent  on  the  accuracy  and  rate 
of  input.  The  amount  of  use  it  can  make  of  this  information  depends  on  how 
much  of  the  old  information  the  target  being  tracked  allows  us  to  use.  If  the 
targets  being  followed  may  only  turn  slowly,  we  may  use  a  long  time  constant 
to  match  this,  the  long  time  constant  giving  a  high  variance  reduction  ratio 
ie  high  gain  in  precision.  "Lively"  targets  only  allow  a  short  time  constant 
which  in  turn  gives  a  low  gain.  Consequently,  high  gain  for  lively  targets 
requires  a  higher  data  rate  (ie  we  make  time  units  smaller).  The  method 
should  allow  us  to  put  figures  to  such  arguments. 

d.  A  long  study  of  the  subject  of  tracking  by  Bordner  and  Benedict  resulted  in 
them  concluding  "to  improve  tracking,  improve  radars".  The  approach  adopted 
for  this  study  should  provide  the  basis  for  quantitative  statements  on  the 
improvements  that  new  radars  could  bring  about,  eg  with  regard  to  data  rate, 
accuracy,  elevation  information,  Poppler  information  etc. 

14.  FUTURE  WORK 

No  tracking  process  is  of  any  value  without  a  means  of  associating  extracted 
plots  with  tracks.  Reference  9  discusses  some  ideas  and  methods  for  such  a 
process.  Any  such  process  requires  knowledge  of  accuracies  of  forecasts 
and  measurements  -  these  will  be  available  if  a  Kalman  filter  is  used  for 
the  tru  Amg  process. 

Current  worn  on  tracking  is  directed  at  studying  the  problems  of  turn 
han'M  ing,  and  then,  having  a  solid  base,  consideration  will  be  given  to  the 
benefits  and  uses  of  elevation  and  Doppler  information  in  a  generalised 
manner. 
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APPENDIX  A 

EQUIVALENCE  OF  KALMAN  FILTER,  LEAST  SQUARES  ag  TRACKERS  AND  LINEAR 
REGRESSION,  FOR  SINGLE  DIMENSION  CONSTANT  ERROR  CONSTANT 
DATA  RATE  CONDITIONS 

Assume  measurements  are  available  as  follows: 

Position  Time 


All  measurements  have  variance  a2 
1.  Kalman  estimation:  *  is 


o  1 


'  ,  M  is  [1  o] 


x’l  h 

nt  _  i 

n 

t. 

r~7T 

1 

X* 

1 

X  i  jo 

n  J  L 

1 

a 

■  xn-l 

i>  . 

Initially,  let  X!  »  z1(  Xj  =  o,  velocity  variance  large 


Hence  X2  is  T  z2  1 

.  Z2'Zl 

<■  t  - 


r o2  o2~ 

t 

;  a2  2 c2 

Lt  t2J 


r  2z2-zi 

j  z2-Zj 


'Sa2 

3a2 

P  *3  = 

t 

5ct2 

2o2 

t 

-24- 


*?J*r**l'  '^f7^rV>TY^^cr<- -'■> \S> »-.=^T ~ ^-.. ^7.  ,v  ,  .-. „-.-*f  vc¥>?s;v*— ;r\. 


u  = 


S  Z2 
az3  +  T 


Zl 

T 


i (23-21) 


2t 


P3  = 


5a2 


6 

,2 


_ 

2t 


_ 

2t 

,2 


O' 
2t2 


X\= 


j(4z3+z2-2zi) 

(23-21) 


2t 


fzsi  2! 

1  3 


P'u  = 


!  a2 

L~ 


2t2 


1 


?T(7zli+4z3+Z2-2z1) 


X,  * 


10 

1 


i 


;i0t(3z4+z3"z2"3zi)j 


2.  Linear  Regression  Estimation 
Wc  assume  observations  are  related  thus: 
x  =  mt+c 


n 


K4  = 


1 

We  thus  wish  to  minimise  s  =  —  T  (z.-mt.-c)2 
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We  do  this  by  finding  setting  both  to  zero  and  solving  for  m  and  c. 


We  get 


nZzt  -  ZzZt 
m  =  -_ 


nZt2  -  (Zt) 2 


It2Zz  -  ZtzZt 
C  =  nZt2  -  (Zt)2 


Hence,  in  this  example,  n  =•  4  and  z's  and  times  are  as  given,  (ie  o,  t,  2t  etc) 


1 


m  =  IoF^3Zl«+z3-Z2~3zi) 


1 


C  =  JQ  (7z1+422+Z3-2z4) 
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. Position  at  time  3t  is: 


A  1  1 

*4  =  jot  (3z4-fZ3-Z2-3Zi)  .  3t  +  fo  C7Zi+4z2+Z3-2z4) 

=  -jg-  (7z4+4z3+z2-2zi) 

and  its  velocity  is  m, 

3.  Least  squares  og  tracker 


x'3  =  z2+ (z2  ~z 1) 

-  5  1 

x3  =  x'3  ^-(z3-x'3)  =  ^(-z1+2z2+5z3) 

*3  =  *2  +  2^(z3-x'3)  =  ^-(-z1+z3) 

«  1 

x'4  =  x3  +  x3.t  =  j(-2zi+z2+4z3) 

-  7  1 

x4  =  X’4  +  yq(z4-x'4)  =  j^(-2z1+z2+4z3+7z4) 

and  x4  =  x3  +  (z4-x\)  =  ■—  (-3z1-z2+z3+3z4) 


Hence  all  three  methods  give  the  same  results,  which  is  not  surprising  since  they 
ail  originate  from  the  same  criterion.  Proofs  which  demonstrate  a  difference 
between  linear  regression  and  least  squares  aS  tracking  must  therefore  contain  an 
error  of  some  sort. 
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APPENDIX  B 

TRANSFORMATION  OF  VECTORS  AND  THEIR  COVARIANCES 

The  problem  discussed  here  is  the  one  which  arises  when  we  transform  a  given 
vector  in  one  plane  to  another  plane.  We  wish  to  find  how  the  covariance  matrix 
which  applies  to  the  original  vector  is  transformed. 

Consider  that  we  have  a  set  of  n  variables,  which  we  group  into  a  vector  Y, 
together  with  a  covariance  matrix  Ry  which  is  nxn  and  contains  the  covariance  of 
the  variables  in  Y.  We  will  transform  Y  to  a  new  vector  X  and  we  now  wish  to  find 
the  contents  of  a  matrix  R  ,  which  is  nxn  and  contains  the  covariances  applicable 
to  X.  x 
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From  the  total  derivative  theorem, 
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Now,  if  the  derivatives  of  Y  are  linear  over  the  intervals  6ys  =  /  rsss 
s  =  1,2,3  .  .  .  n,  we  may  use  (B. 1)  and  (B.2)  to  give: 
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n  n  3 x.  3x,  ,  .  m 

=  V  f  (lit  -  t  6y.  6y  \ 

jii  s=i  ayj  3ys  v  m*“ *  q:i  J<1  sq  / 

These  last  two  steps  are  valid  for  these  summations. 

The  last  part  (ie  following  limm)o])  in  the  expression  for  rxik  is  the  definition  of 

the  covariance  of  y.  and  y  hence: 

j  s 

n  n  .  3x.  3x,  . 

rxik  ■  j=i  sM  Vyd  <B’3) 

Ke  have  therefore  established  the  relationship  between  R  ,  the  covariance  matrix  of 
V,  and  R  ,  the  covariance  matrix  of  X.  y 

This  may  be  written  in  matrix  form  as:- 

R  =  AR  AT 
x  y 

3x. 

where  A.  .  =  -r— 


(B.3) 
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APPENDIX  C 


ESTIMATION  OF  MOST  PROBABLE  POSITION 
The  joint  probability  density  function  is  given  by  (see  text  9  a.) 


p  =  ex? 


(X'2-X)2  (X2-X): 

2a'  "  2r 


exP  ^ 


The  value  of  x,  x  which  maximises  p  is  given  by  the  solution  of  =  0 


Hence  £  =  fV*  +  ^  =  0 
dx  a’  r 


x  =  rx'2+a'x2 


a'+r 


We  also  wish  to  know  the  variance  of  x  which  we  call  a2 


Let  h  =  ,  k  =  -2— 

a'+r  a'+r 


Then 


x  =  hx'2  +  kx2 


Let  us  assume  an  error  of  6x'2  in  x'2  and  6x2  in  x2. 

A  /s 

Then  the  error  in  x,  <5x  =  h.6x'2  +  k.<5x2 

The  variance  of  x  is  defined  as  the  mean  value  of  (Sx)2  over  many  samples. 

Thus  a2  =  mean  (hz(6x'2)2  +  2hk<5x'25x2  +  k2(6x2)2). 

We  assume  that  measurement  errors  are  uncorrelated,  ie  that  6x'2  and  6x2  are 
independent;  we  also  assume  that  they  have  zero  mean.  Hence  mean  (6x'2Sx2)  =  0 


Thus  a2  =  h2a'  +  k2r 

r2a'  +  r(a')2  _  ra' (r+a1) 

(a'+r)z  _  (r+?.')2 


ra1 

r+a' 


i 

\ 


\ 
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ESTIMATION  OF  MOST  PROBABLE  VELOCITY 


The  method  of  estimating  velocity  is  basically  the  same  as  that  for  estimating 
position  with  one  important  difference.  Our  two  estimates  of  velocity  are: 


X'2-Xi 


X2-X1 


It  is  clear  that  the  two  estimates  are  related  since  they  both  include  data  from 
the  same  point.  If  we  call  these  estimates  Vi  and  V2  respectively,  and  assume 
variances  si2  and  S22  and  covariances  S122  (these  values  are  calculated  in  C.2) 
then  the  joint  probability  density  function  may  be  shown  co  be  given  by: 


2tts 


where 


.  _  S212 
'  SlS2 


-(Vi-v)2  (v?-v)2  +  2p(y-vQ  (v-v?) 

Sl2  S22  S!S2 

2 (1 -p2) 


We  wish  to  find  the  value  of  v  which  maximises  p  ie  the  value  of  v  to  satisfy 

^-  =  0,  where  J  is  the  argument  of  the  above  exponential  function. 

2(vi-v)  +  2(v2-v)  +  2p(2v-vi-v?) 
dJ  =  si2  s22  sis2  =  n 

dv  - 


2  (1-PZ) 


Hence 


=  s22v1  +  s12v2  -  s122(vi+V2) 

Si2+S22-2si22 


(C.l) 


Now  we  know  that  V2  =  X2  ji-  and  Vi  =  X — 

t  t 

which  may  be  expressed  in  matrix  form  as: 

V  =  FX  ie  r  " 

r  •]  r  1  1  1  Xl 

'  Vll  !"t  t  0  x'2 

'  !  "  j  1  l 

Lv2.  |-T  0  j^x2 

If  we  consider  errors  in  each  element  of  X,  we  may  construct  a  matrix  out  of  them: 


■  5x*2 
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Z.T'^7  -.  •  jcvnrrwv^i^rf^Mjqrj"  ;<c.  r  Tl» 


Now  the  variance  in  xj  is  the  mean  value  over  many  samples  of  (6xj)2,  and  so  on 
for  x * 2  and  x2.  The  covariance  of  xj  and  x'2  is  the  mean  over  many  samples,  of 
(Sxi .  <Sxf2)  •  We  may  represent  these  variances  and  covariances  in  a  covariance 
matrix,  which  contains  the  mean  values  of  the  elements  in: 


fixi  “ 
6x'  2 
6x2 


[6xx  6x'2  <Sx2  ] 


ie  the  covariance  matrix  elements  are  the  mean  values  of  the  elements  in: 


(5xi)2 

6xi6x*2 

6x16x2 

6x'2<$Xi 

(6x'2)2 

6x' 26x2 

6x2<5xi 

6x2<5x'2 

(6x2)2 

Now  the  covariance  matrix  associated  with  X  is 
a  p  o 

p  a'  o 

o  o  r  i 

where  a  is  the  variance  of  xi,  r  is  the  variance  of  X2,  p  is  the  covariance  of  xi 
and  x'2,  and  a'  is  the  variance  of  x'2-  Note  that  the  covariances  associated  with 
X2  are  zero,  since  we  assume  that  errors  in  this  measurement  are  not  related  in  any 
way  to  errors  in  previous  measurements. 

We  may  apply  (B.3)  of  Appendix  B  to  calculate  the  covariance  matrix  of  V,  which  is: 


V  S122l 

_S122  S22  _ 

a+r 

t2  - 

l\’e  may  apply  a  similar  approach  to  estimate  p  ie 

J"*  A  "I 

given  x*2  =  [1  t]  \  x\  \ 

«  i 

! 

vi  , 

with  covariance  for 

*1 

of 

vlj 

j"a  b] 

lb  dj 

t  ■* 

and  applying  (B.3)  we  get, 

for  covariance  of 

IV 


x'2' 
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a  p 


a  a+bt 


p  a';  j  a+bt  a+2bt+t‘-d 

J  L  j 


P  =  a+bt 


We  may  thus  combine  this  result  with  (C.2)  and  (C.l)  to  give: 


(x' 2~xi) (a+r+bt)  +  (X2-X1) (t2d+tb) 


t(a+2bt+t2d+r) 


a+r+bt  _  1_  (t2d+bt) 

t(a+2bt+t2d+r)  t  t(a+2bt+t2d+r) 


We  may  thus  rearrange  (C.2)  to  give 


x*2-xi  (  (bt+t2d) (x2-x'2) 
t  t(a+2bt+t^d+r) 


=  Vl  +  (b+tdKxz-x^) 

a+2bt+tzd+r 


(C.3) 


vi  being  the  previous  estimate  of  velocity. 


We  now  move  on  to  calculate  the  variance  of  v  which  we  will  call  s2. 


V  =  V!  +  |  (X2-X21) . 


Thus  we  are  performing  the  transformation: 


v-  f| 


i]  r x2  "i 


In  order  to  find  the  variance  of  v,  we  need  to  determine  the  covariance  matrix 
associated  with 


We  assume  that  xz  has  variance  r,  and  is  uncorrelated  with  X2*  and  vi  (ie  that 
errors  in  measurement  xz  are  not  related  to  errors  in  forecast  and  velocity 
estimates).  Thus  all  that  remains  to  be  estimated  is  the  covariance  matrix  for 


V; 


WUB'MUML'&J  V.KiVJJi  satMJ-WAWCTM  yarjfsjji  c«j.<u«-i»w.gfi  .u  )«J«  •ami 
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Summary 


The  best  estimate  of  position  has  btJn  shown  to  be  given  by: 


rx'  +  a'x 
2  2 


and  its  variance  is: 


where 


a+2bt+t2d 


The  best  estimate  of  velocity  is  given  by: 


(b+td) (x  -  x  ') 
2  2 

v  =  v  +  - - - i — 

1  a+2bt+tzd+r 


and  its  variance  is 


d(a+r)-b2 


a+-2bt+t2d+r 


APPENDIX  D 


APPLICATION  OF  RESULTS  IN  APPENDIX  B  IN  POLAR  TO  CARTESIAN  TRANSFORMATION 
Consider  the  following  polar  to  Cartesian  transformation  (ie  from  r,  8  to  x,  y) 
x  *  r  sin  0 

)  a  r  COS  8 

(Note  that  e  is  interpreted  in  the  nautical  sense  ie  0  is  a  bearing  which  increases 
in  a  clockwise  direction  from  the  positive  y  axis.) 

The  covariance  matrix  associated  with  r  and  0  is  assumed  to  be: 


: °2rr  0 

!  j  ie  r  and  0  are  independently  variable 

5  1 

0  0Z 00  ! 

I,  J 

(as  would  be  the  case  when  r  and  0  are  determined  from  a  radar  auto-detection 
system) . 

We  may  relate  the  problem  to  the  argument  in  Appendix  B  by  considering: 

I  x 

X  as  : 

. 

r 

Y  as 

0 

The  transformation  of  covariances  given  by  (B.3)  involved  a  condition  of 
linearity,  this  holds  if  sin  oqq  =  o@ q  which  is  valid  for  all  radars  the  author 
is  aware  of. 

We  will  denote  the  covariance  matrix  applicable  to  X  as: 


02 

02 

:  XX 

xy 

Rx  = 

o2 

a2 

xy 

yy 

Hence,  by  substitution  into 

(B.3) 

i 

f 

f 


2 


(since  o2r0  =  o2er  =  0) 


Hence  Rx 


j  o2rT  sin20+r2o2g0cos20 
(a2rr-r2c2gg) sin0cos6 


(o2rr-r2a20e)sin0cos0  ; 
a2rrcos20  +  r2a2g0sin20  ! 

i 

j 


This  is  the  covariance  matrix  for  the  transformed  vector  X  which  we  set  out  to 
f  ind . 

It  is  interesting  to  note  that  in  general,  the  co-ordinates  after  the  trans¬ 
formation  (x  and  y)  are  notoindegendent.  The  singular  cases  where  they  are 
independent  occur  for  0  =  0°,  90  ,  180°,  . . .  or  for  r  =  o^/oqq.  These  special  cases 
are  easily  explained  as  follows:  the  equal  probability  contours  for  bivariate 
Ciaussian  distributions  are  ellipses:  if  the  axes  lie  in  the  directions  of  the  axes 
of  the  frame  of  reference,  then  the  covariance  terms  are  zero:  hence  the  independence 
of  x  and  y  errors  for  0=0°,  90°  etc.  When  r  =  ott/cqq,  these  ellipses  reduce  to 
circle'  which  may  be  considered  as  limiting  ellipses  whose  major  and  minor  axes 
lie  along  the  axes  of  the  reference  frame. 

It  is  worth  pointing  out  t iat  the  covariance  matrix  could  have  been  derived  by 
considering  the  geometry  of  the  ellipse  and  the  rotation  required  after  trans¬ 
formation.  However,  this  is  complicated,  particularly  for  a  transformation  from  one 
3-D  frame  to  another,  when  it  is  not  just  a  simple  translation  or  rotation  of  axes-. 
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XY  FIRST  ORDER  TRACKER  -  PROGRAM  HARD  COPY 


;=3»^TKf  J»  ;f*QY  ^J^KKTI 


jBjur;:- — y'33R?^»s*» 1  >«wiTOrf,i(J'?'ir'  7 
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♦  lODUM!  T I  0/PRI>JT 

♦  10  DUN  I  T2Q^READ 
*XEQ 

♦  storage/ 40 on/ 15000 

♦CHAINl 

*ALGJL 

'BEGIN  INTEGER'  I.  J#  MEASURE#  STATE.  Mt»N#0»0LDX* 

'ARRAY  'K(lt».U!).l’n5?*!t?)«^1*?»IJ!)» 

PC  1  :  4#  1  :  4>#MC  1:  2#  1:  4)»M|(  I:  4#  1:  ?>• 

PH  I  (  1  •  4»  1:  A)#  KC  |:  4#  1:  2)#  PMTC  l:  4#  1:  2)  #1)  UTXC  1:  4.  1:  1  ># 

OUTZC  1:2#  1:  1  )  * 

'REAL'  T#  TIME#  DEL  TAT#  CHAN  GFJ.  F  INI  SH»  FX#XPJT»  CHANGE  OF  XD)T» 

V.’Y,  YDJ  T»  CHANGE  OF  YOU T»  X O#  Y fl#  VARR#  VARTKE7A,  RNO  I  SF,  THFTANO?  SF. 
Ul,  U2»  VARIANCE,  NO?  SF.  RANGE#  PEARI  N  G#  SI  NTH  ETA#  CO?  THETA*” 

'COMMENT'  X#Z  AND  CUPY£  ARE  COLUMN  VECTORS, 
p  AND  COPYR  APE  ?*?  MEASUREMENT  COVARIANCE  MATRICES* 

H  IS  A  4*2  MATRIX#  AND  IS  ESSENTIALLY  THE  DAMPING  FACT!)  R* 

M  IS  THE  MEASUREMENT  MATRIX  AND  IS  2*4. 
i»i T  IS  THE  TRANSPOSE  3F  M. 

TRACK  IN  X»Y#  GENERATED  IN  X,  Y  H  TH  CONSTANT  DATA  PATE. 
MANOEUVRE  AT  TIME  CHANGE.  FINISH  AT  TIME  FINISH. 

KALMAN  FILTFR  TRACK FR  WITHOUT  PLANT  NilSE  TERMS  e 


r  « 

I  l 


'PROCEDURF  '  TRANSPOSE!  PHI#  PHI  T#  MI  ,N>  « 

'INTEGER'  MI »  N  « 

/ARRAY  '  PHI#  PHI  T  c 
'BEGIN  INTEGER'  l»l  “ 

/ARRAY  '  COPYPHIC  l:MI#  1*N)  « 

'F3R'I=  1  'STEP  '1  'UNTIL  'MI  '|>J  ' 

'F  JR  'J=  1  'STEP  '1  'UNTIL  'N  'DO  7 
CJPYPHIC  I#.J)  =  PHIC  I#  ))  T 

'COMMENT'  PHI  COPIED  SO  THAT  TRANSPOSE  CAN  RF.  ASSIGNED 
TO  ORIGINAL  MATRIXCONlY  PJSSIRLF  FOP  SQUARE  MATRICES)  r 
'FJR  '1=  1  'STEP  '1  '(N  TIL  'MI  'DJ  ' 

'F  J  R  '.J=  1  'STFP  '1  'UN  TIL  'N  'DJ  ' 

PMITCJ»I)'  =  C)PYPHICI#J)  * 

'END 'TRANSPOSE  T 


'PPJCEDURF'  MATRIXINV-C  A,  P.MI  )  c 
/ARRAY'  A#  o  «• 

'INTEGER'  MI  * 

/oc-pi\)  =:f AL  '  DET  r 

/r  jw  ^  r\)T  '  TJ  I  N  v-ERT  A  2*2  MATRIX  * 

'I  F  'MI  'NF  '2  'THEN  PFfjIN' 

IvRI  TETC  1  A#  'C  f  'C  ')  'M  NE  2')')  c 

'CO T J  '  STOP  «■ 

'E\jf>'  c 

DFT=AC 1 .  1 )*AC  2#  2) -AC  1 »  2>*AC  ?»  J) 

PC  1 »  1  )  =  *(  2,  2) / DE T  T 
PC  !•  2 )  -  C  -  AC  1#  2>/DFT)  «• 

RC  2.  1  )  =  C  -  AC  2#  !  )'  DET  )  r 
Q(  2#  2 )  =  AC  1#  1  )/PET  «• 

^TJP:  'FND'  MATPIXINV  « 
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't'!{  JCF'PURE  •  1ATRI  XAPOC  A.  U,  C.M  I  .  M  >  -' 

'’ARRAY'  A,  H,  C  <r 
'I  M  TFT.F’.R  '  Ml.M  «■ 

'COMM  EM  T '  TM  E  SUM  OF  THF  MAT  Pi CKS  IS  STJRED  I M  THE  ARRAY 
C.  A.  R,  C  ARE  MI*M  <r 
'BEDIM  l  M  TE HER  '  l.J.P  c 

'FOR  '1  =  1  'STFP  '1  'IN  TIL  'MI  'Di  ' 

'FO P  '.)  =  1  'STEP  '  1  'UMTIL  'M  'DO  ' 

CC  I » J  >  =AC I»J)  +  B(!»J>  * 

'  FN  D  '  MAT  RI  XApD  * 

'PROCEDURE'  MATP[XSUP(A,R,r,MI.\n  « 

'ARRAY  '  A»  R»  C  f 
'INTEGER'  MI.M  «■ 

'COMMENT'  THE  DIFFERENCE.  OF  THE  TWO  MATRICES  IS  STORED  I  'I  THE 
ARRAY  c.  A.R.C  APE  Mt*M  « 

'REG!  M  IMTEGER'  l.J  « 

'FOR '1  =  1  'STEP  '1  'DM Tit.  'MI  'DJ  ' 

'FOR'  l=  1  'STEP  '1  'DM  TIL  'M  'DJ  ' 

CC  I»J>=AC  I...D-RC  I.J>  *r 
'EM  D  '  MATRIX  SUB  «“ 

'PROCEDURE'  MATRI  <MDLT(A»«»C»MI,  M,  p)  «■ 

'ARRAY'  A,  R.  C  ? 

'I  M  TEGER  '  MI.  M,  R  «“ 

'COMMENT'  THE  PRJnHGT  JF  THE  TWO  ARRAYS  ■$  STJPED  IM  THE 
ARRAY  C.  •*■■*  MULTIPLY  «Y  A  SCALAR  THE  SECOMO 
MATPIX  MU  .  HAVE  THR  VALUE  OF  THE  SCALAR  AS  ITS  PI  rtGJ vAL 
WITH  HE  PEST  JE  THE  EL  EM  EM  TS  AR  MjUfHTS.  A  IS  M*  M .  R  T  S  N*  P» 

C  IS  **P.  CJPIES  OF  I’M  PUT  ARRAYS  ARE  MADE  IM  ]Rt’EP 
that  AM  ARPAY  MAY  re  used  BOTH  AS  AM  IMPUT  amt 
OUTPUT  PARAMETER  « 

'RF.GIM  IMTFGER'  T.J.X  * 

'ARRAY'  COPY  AT  1  :M  I  .  1  !  M  )  .  CO  PYR(  1  t  M  »  1  !  P)  «“ 

'FOR '1=1  'STEF  '1  'IN TIL  'Ml  'DJ  ' 

'F  JR  '0=  1  'STEP  '1  'UMi  II  'M  'DJ  ' 

COPY  AC  I.  J)  =  A(  I.  J)  «■ 

' FO  R  ' I  =  1  'STEP  '1  'UMTIL  'M  'DJ  ' 

'FJR'0=1  'STEP  '1  'UMTIL  'P'DJ  ' 

COPYec  !*.!>  =  RC  I.  J>  c 

'FO  R'f  =  1  'STEP  '1  'UMTIL  'MI  'TV  1  ' 

'  F  J  E  '•  J  =  1  'STEP'!  'UMTIL  'P'DJ  ' 

'REGIM  '  C(  I » .1 )  =  0  c 

'F  1P'X=  1  'STEP  '1  'UMTIL  'M  'DJ  ' 

CCI.  H=CJPYAC  I.  X>-*=CJPYRCX,  J)  +  CC  I.  J)  «• 

'EM  D  '  c 

'EM  n '  MATRI  XMILT  c 
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'PROCEDURE  7  DUMPCA.MI.N)  «■ 

-'ARRAY  '  A  * 

'INTEGER'  M I , N  « 

'■BEGIN)  INTEGER  'I.  J  * 

'FOR '1=1  'STFP'l  'IN  TIL  'MI  'DJ  ' 

'BEGIN)  ' 

'FOR '0=1  'S^EP'l  'UN TIL  '\J  'DO  ' 

l\RI  TEC  1  0.LAYOUTC  'C  'S-M  00000.  ODD')  ')»  AC  I»J>>  * 
'END'  * 

Nl  EKL I  NIC  1  0»  4)  « 

'ENJO'  DUMP  f 


'REAL  PROCEDURE '  RANDOM  ? 

'BEGIM  INTEGER'  CALC  * 

'COMMENT'  OLDX  MUST  RE  DECLARED  GLOBALLY  * 
0LDX=?55*0LDX  $ 

CALC=  EN  TI  F  PC  OLDX/  131  0  71)  * 

OL  DX=OL DX -  1  3 1  0  7 1  *  CAL  C  S 
BA  >1  Di.VM= JL  DX/131071  ? 

'END'  RAN  DJM  « 

'PROCEDURE'  GAUSSIANCNJI  SE.  VARIANCE)  « 

'REAL  'NOISE,  VARIANCE* 

'BEGIN  REAL'  TWO  PI  * 

TWO  PI  =  6  PS31R  ? 

Ul  =  RANDOM  « 

U2=  RAN  DOM  « 

NO  I  SE=  SORTC  -  2*  VAR!  A.NCE*  LNC  U1  )  >*COSC  TWO  PI*  U2>  * 
'END'  GAUSSIAN  * 

'PROCEDURE 'SET  P  MATRIX  * 

'BEGIN' 

RC  1.  1)  =  St  N  TB  ETA*  SIN  THETA*  VARR+E  CP*  1)*ZCP.  1  )*  VARTMETA 
R(  ) .  2>  =  R<  P»  1  >  =  SI  N  THETA* CO  STHETA*  C  VARR- 
RANGE+RANGE*  VARTHETA)  * 

PC  2.  P)=COSTHETA*C'JSTHETA*  VARR 
*'C  l»  1  )*^C  1,  1  )*VARTHETA  « 

'END'  SET  R  MATRIX  * 

'PROCEDURE'  NOISEPLOT  * 

'BEGIN  COMMENT'  TJ  SET  L  MATRIX  WH EN  NOISE  ADDED  * 
THETAN  JISE=RNOISE=0  1 
GAIJSSIANC  RN.JI  SE*  VARR)  * 

GAUSSIA.NC  THETAN  01  SE.  VARTMFTA)  * 

PA  g  r-E=  SR  PTC  EX*  EX  *  WY*  WY )  ♦  RNOl  SE  * 

PEARIN  G=  ARC  TAN  <  EX  /  wY )  *  THE  TAN  j  I  SE  * 

SI  NTHETA=SINC  REARING)  «  CO  STH  ETA=  CO  SC  REARI N  G)  ? 

Li  1 »  1  )  =  RAN  GE*  SIN  THETA  =  li  P.  !  )  =  RAN  GE*  CO  STM  ETA  * 

SET  R  MATRIX  * 

'END'  NOISFPLOT  * 
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'procedure'  cleamplot  <? 

'PPG!  V)  ' 

RA'\I  GE=  SOETC  EX+  EX  +  WY*  U'Y )  * 

9EARIMG=ARCTAMC  EX' WY)  * 

SIM  THETA=  SIM(  «EARI  M  G)  c  C  J  S  TH  E  TA  =  CO  SC  R  E A  R I  M  G )  * 

£(  l>  !  )=EX  «  X(  2.  1  >  =  G. Y  <“ 

SET  R  MATRIX  « 

'EMD'  GLEAM  PLOT  « 

'PROCEDURE  '  TRACK  (MEASURE.  STATE.  PHI.  M.MT.  P.  X,  -i,  R)  e 
'ARRAY  '  PHI,  P,  X.  L,  R.M  T»M  « 

'INTEGER'  MEASURE.  STATE  * 

/bEGTM  ARRAY  '  M  PC  1  S  MEASURE,  l:  STATE).  PM  I  TC  1 :  STATE.  1:  STATE). 

MtViTC  1 :  MEASURE.  1  t  MEASURE)  >  PRED-2C  1 :  M  EASURE.  lit). 

E»ROPCOV(  1 :  M  EASIJRE,  1  {MEASURE)  ♦  CURRECTIOMC  l:  STATE.  Jj  1  ), 

K M P(  1 :  STATE.  \  i  STATE)  * 

TRAM  SPO  SEC  PHI »  PHI  T.  4,  4)  * 

MATRI  XMI.LTC  PH  I  »  X.  X.  4,  4.  1  )  ? 

MATRIXMLLTC  P»  PHI  T.  P.  4,  4,  4)  « 

MATRIXMIJLTC  PHI,  P>  P,  4,  4,  4)  ? 

MATPIXMI/lTCM.  P,  MP»  2,  4,  4)  « 

M  A  TRI  Xn  IJL  TC  P.  M  T.  PM T.  4,  4.  2 )  S 
MATRIXMIILTCM,  pmt.mpmt.  2.  4.  2)  - 
MATRIXADDCMPMT,  R.MPMT.  2.  2)  * 

MATRIXIM  VCMPMT,  ERRO  RCO  V.  2)  S 
MATRI  X.'HLTC  PM  T»  ERRJRC'j  V»K.  4,  2.  2)  * 

MATPIXMI.LTCM,  X.  PPED^,  2.  4.  I  )  * 

MATRI  X S1  'PC  PRED 2, PPED4,  2,  1  )  * 

MATPIXM'LTCK,  PPEH/,  OjPRECTI'JM.  4,  2.  1  )  «“ 

'Comm em t '  pred2  is  mjw  cm*x-2>*k  «• 

v)AT»IXSHRCX.CJt?PErTIOM,X.  4,  1)  <r 
MATRI  XMULTCK.MP.KMP,  4,  2»  4>  «r 
MATRIX  SI.IPC  P.KMP,  P,  4,  4)  * 

'COMM  EM  T  '  FJR  A  4  VARIABLE  STATE  VEC  TJ  RC  STA  TP=  4) , 

AMD  A  TWO  VARIABLE  MPASUREMEMT  VECT'JRCMEASI.tPE=2) , 

X  IS  4*  1 ,  £  I  ?  2*  1 .  P  IS  4*  4.  M  IS  2*  4.  PH  I  IS  4*  4,  X  l  ?  &*-P  ? 

' FM  D '  TRACK  « 

MC  1.  1  )=MC  2.  3 )=M TC  1 »  1  >=MTC  3.  2)  =  1  f 

Mf  1. 2)  =  'iC  1 . 3)=M(  1 , 4)  =MC  2.  1  )=M(  2*  2>=M(  ?»  4>=MTC  1.  ?)=MTC  2»  P)=MTC  2.1)  = 
TC  3.  1  )  =M  TC  4,  1 )  =M  7C  4,  P>  =  0  T 
'F.JR'Ts  1  'STEP  '1  'UM  TIL  '4'm  ' 

'f  )»/  |=i  'STEP  '1  'UM  TIL  '4'DJ  ' 

'REGI  M  ' 

PH  I  C  !>..!)=  'I  F  /X  'EO'.I  'THEM  '1  'ELSE'O  ^ 

PC  I .  J  >  =  0  T 
'EM  D  '  « 

° T A T E =  4  c  MEASURE=P  «“ 

'C  FM  T  '  STATE  VECTOR  IS  4"  VARI.aol  E.  MFA  SI  'F/EM  E.M  T  VECTJR 
(  <;  ?-  VAR  I  api.  p  <f  ' 


C  r’A (:*•  S> 


X0=  READC  20 )  «  Y0= READC ?0>  « 

XDJT=R£AOC?0)  «  YD  J  T=  PEAPC  ?f)  >  c 

CHANGE  OF  X  r>3  T=  READC  P0  >  «  CHAM  GE  J  F  YDJ  T=  READC  Pfl )  * 
CHAM  GE=  READC  20 >  «  F  I  M  I  SH  =  READC  20 )  *  DEL  TAT=  REA  PC  ?n  >  " 
VARR=READ(  20)  c  V/APTH ETA=  READC  ?0>  «  JLOX=  READC  PC >  5 

mo i  se=pf:ad(  po>  r 

'•'RI  TETC  10*  '((  'C  '>  'X  '*  'Y  '*  'KALMAN  '*  / TRACK  £R'C  'C  ')  >  ')  T 
I  MI  TI  ATE  TRACK :EX=XO  «■  '.•.Y=Y0  « 

'IE'.NOISE  'EG'  1  'THEN  '  NJISEPLJT  * 

'I  F  'NO  I  S£  'EG  '  0  'THM  '  C.LEANPLJT  « 

'  <(1*  1 >=£<  1»  I  >  *  XC  3*  1 )=£( 2*  1  >  ? 

XC  {?„  1  )  =  X(  4.  1  >  =  0  * 

DLMPCX.A,  1)  *  DUMPC<SP»1>  * 

PM»l>  =  pC1»l>  *  PC  3*  3>=RCP»  2>  * 
pc  l,  ro  =  Pc  3.  n  =  R(  i»  ?)  <• 

PC?.  Pt  =  P(  A,  A)=  i  «• 

'COMMENT /  TRACK  IS  NOW  PRICED  e 

'FOR'  T=  DEL  TAT  'STEP  '  DEL  TAT  'DM  TIL'  CHAMRF  'DJ  ' 
'BEGIN  ' 

EX"  X  0+  X  DJ  T*  T  *  WY= Y  0+-  Y  DJ  T+  T  ? 

'IF'  NOISE  'EG'  1  'THEM'  NO  I  SEPLOT  « 

'IF'  NOISE  'EG'  0  'THEM'  CLEA.MPL'IT  f 
'COMMEN T '  NOISE  ADDED  IN  NO  I  SEPLOT  « 

PHIC  1»P>  =  PHI(3»  A)  =  DEL  TA  T  * 

TRAPKCP*  A,  PHl»M»MT»  P»X.£»  R>  « 

HUM  PCX,  A*  n  ?  DUMPCZ,  2*  1 )  ? 

TIME=T  * 

'END'  t 

X  0  =  X  0  +  X DJ  T*  TINE  ?  Y0=Y0*YOOT*  TIME  ? 

X[>JT=XDJT+ CHANGE  OF  XDJT  ? 

Y DO  T=  Y DJ  T+  CH AM  GE  OF  YDJ  T  P 
FINISH=FI.MISH-TIME  * 

'FO  R  'T=  DEL  TAT  'STEP  'DEL  TAT  'UM  TIL  '  FINISH  'DJ  ' 

'BEGIN  ' 

EX=X  0+X  DJ  T+=  T  *  WY= Y  0+ YDJ  T*  T  * 

'IF'  NOISE  'EG'  1  'THEN  '  NO  I  SEPLOT  « 

'IF'  NOISE  'EG'  0  'THEM'  CLEAN  PLOT  ? 

PHIC  1»?)  =  FHI<  3*  A)  =  DEL  TAT  * 

TRACK  C  2*  A*  PH  I »  M,  M  T»  P»  X»  £  *  R)  S 
DUNPCX*  A,  1  )  «  DUMPCZ.2,  1)  S’ 

'END'  c 
'END  ' 
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POLAR  FIRST  ORDER  TRACKER  -  PROGRAM  HARD  COPY 
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♦  [  JOIN!  T1 IKPRIN  T 

*  I  JOINI  TPO/READ 

*  X  EG 

*STO  RAGE' 4(100' 1  500P 
*chain 1 

♦  AlGOL 

'BEGI  N  t  N  TEGER  /  I .  J.  MEASURF.  STA  TE«  M  t .  N .  3  ,0L  DX  * 

'ARRAY  '  XC  IS  4,  IS  1  ).  RClSP.lSg),  ZClSP.lSl),  COPYRC  1  5  P»  1  8  P) » 
CGPYZC  1 S  ?»  IS  1),  P(  is  4.  is  4),  rt(  ISP#  IS  4).MT(  is  4,  Jjp>. 

PHI (  1  s  4.  is  4>,  K<  IS  4,  is  ?) .  PMTC  IS  4»  l :  gj.OUTXc  1  s  4,  i :  i  ) , 

UU7ZC  is?,  is  1)  * 

'REAL.'  T»  TIME.  DEL TAT.CHANGE.  FINISH,  FX.XDJT.CHANGF  OF  XPJT. 

WY.  Y  f>J  T*  CHANGE  0  F  YPJT.  Xfl.  Yfl,  VARR,  VAPTHETA.  RNGI  SE.  THETANGT  SE, 
Ul.  UP.  VARIANCE. NJI SE? 

'CUM o') EM  T '  X»Z  AN  D  COPYZ  ARF  COLUMN  VECTJPS* 

R  AND  COPYR  ARE  2*  P  MEASUREMENT  COVARIANCE.  MATRICES. 

K  IS  A  4*2  MATRIX.  AND  IS  ESSENTIALLY  THE  DAMPING  F at TJ P. 

M  IS  THE  MEASUREMENT  MATRIX  AND  IS  2*4. 

MT  IS  THE  TRANSPOSE  OF  M. 

TRACK  IN  P.  THETA,  GENERATED  IN  X.Y  WITH  CONSTANT  DATA  RATF. 
MANOEUVRE  AT  TIME  CHANGE*  F  I N I SH  AT  TIME  FINISH. 

KALMAN  FILTER  TRACKER  WITHOUT  PLANT  NOISE  TERMS  * 


'PROCEDURE'  TPANSPJSEC  PHI .  PHI  T.MI »  N  >  * 

'INTEGER'  MI.N  * 

'ARRAY  '  PHI.  PHI  T  * 

'BEGIN  INTFGEP'  I.J  ? 

/ARP AY'  COPYPHK  1S.MI,  1S.N)  ? 

'FOR  '1=  1  'STEP  '1  'UNTIL  'MI  'DO  ' 

'FOR  'J=  1  'STEP  '1  'UNTIL  'N  'DJ  ' 

COPYPHK  I.  J)  =  PHIC  I..J)  c 

'COMMENT'  PHI  COPIED  SJ  THAT  TRANSPOSE  CAN  RE  ASSIGNED 
TJ  ORIGINAL  MATRIXCONLY  POCSIRLF  FOR  SQUAPE  MATRICES)  * 
'FO p  'I  =  1  'STEP  '  1  'UNTIL  'MI  'DO  ' 

'FOR  '0=  1  'STEP  '1  'UNTIL  'N  'DJ  ' 

PHI  T(J.  I)  =  CJPYPHI  (I.  J>  * 

'END 'TRAN SPJSE  * 


'PPJCF.DUPE  '  MATPf  XI  N  V<  A,  R.MI  >  « 
/ARRAY  /  A,  R  S 
'INTEGER'  MI  « 

'REGIN  REAL'  DET  « 

'COMMENT'  TO  INVERT  A  ?*P  MATRIX  * 

'  I  o  'M  I  'N  F  '  ?  '  TH  E  M  BE  G  i  N  ' 

WRI  TET<  1  0.  '(  (  'C  ')  'M  NF  ?')')  ? 
'GJ7 ]  '  STJP  « 

'END'  « 

DFT=A(  1,  1)*A(  2,  2) -AC  1.  P)*A(P.  n  c 
R(  1 .  1  )  =  A(g,  ?)  /DET  ? 

R( 1 »  P)  =  ( -A(  l  ,  g) / DET)  ? 

BC  ?.  1 )=( -AC  2,  1 ) 'DET)  ? 

PC  ?»  ?>=A(  1,1)/  DET  «• 

STOP:  'END'  MA1PIXINV  * 


C  PA  {,F.  ?) 


'PRJCEPURE'  MATPI  XAPPC  A,  R.  C>MI ,  \) )  « 

/ARRAY  /  A,  A,  n  * 

'I  M  TEGER  '  MI .  M  <f 

'C'JMMEM  1  '  THE  $tM  JF  HE  MATRICES  tS  STJREP  I M  THE  ARRAY 
C.  A,  R.C  APF  Ml*M  «“ 

'BEG!  M  IMTEGEP  '  I.  I.  P  * 

'F0P'T=1  'STEP  '1  'UMTIL  'MI  'PJ  ' 

'FOR '0=1  'STEP  '  1  'IN TIL  'M  'DJ  ' 

C(  I  »  .J  )  =  AC  I  *.])■*•  P(  I »  J  )  <r 
'EM  0  '  MATRIXADP  e 

'PROCEDURE  '  MATRIX  SlJP(A*  R »  C »  M I  *  M  )  * 

'ARRAY  '  A#  R,  C  * 

'IMTEGEP'  Mt>M  « 

'COMMENT'  THE  DIFFERF-MCE  OF  THE  TWO  MATRICES  IS  STJPED  I M  THE 
ARRAY  C«  A, R.C  ARE  Mt*M  * 

'RFGl M  INTEGER'  I.J  * 

'FUR  '1=  1  'STEP  '1  'i.N  TIL  'MI  'DJ  ' 

'FOR  '.J=  1  'STEP  '1  'I.N  TIL  'M  'D3  ' 

Cf  I»J>  =  A<  I».J)-R(  I#.J)  T 
'E'JP'  MATRIX  SUB  * 

'PR  JCEDI IRE  '  MATRIXMLiLTCA,  B.  C.-MI.  M»  P)  * 

'ARRAY  '  A,  p»C  c 
'IMTEGEP'  i'll.  M.  P  <■ 

'COMM EM T'  THE  PRODUCT  OF  THE  TWJ  ARRAYS  IS  STORED  IM  THE 
ARRAY  C.  TO  MULTIPLY  BY  A  SCALAR  THF  SECJMD 
MATRIX  MUST  HAVE  THE  VAlUE  OF  THE  SCALAR  AS  ITS  PI  AGO  M  al 
WITH  THE  REST  JF  THE  ELEMEMTS  AS  NOUGHTS*  A  IS  M  ♦  M »  R  IS  M*  P» 

C  IS  M*P.  CORIES  JF  IMPUT  ARRAYS  ARE  MARE  I  \l  JRPER 
THAT  AM  ARRAY  may  BE  USED  BOTH  AS  AN  IMPI.IT  AMD 
OUTPUT  PARAMETER  * 

'REGIM  IMTEGEP'  t»U,  X  * 

' ARt\AY  '  CJPYAC  1JMI,  1JM)»C.]PYRC  1 8  M »  1 :  P>  p 
'FOR'I=  1  'STEP  '1  'IN  TIL  'MI  'DJ  ' 

'F  J R  'U=  1  'STEP  '1  '"M  TIL  'M  'DJ  ' 

CJPYAf  I,  J)  =  AC  I.  I)  «■ 

' F  J  R  ' I  =  1  'STEP  '1  'i.N  TIl  '.M  'DJ  ' 

'F’J  R  '  1=  1  'STEP  '1  'i.N  TIL  'P  'DJ  ' 

COPYRf  I  * .1 )  =  Rf  I »  J)  * 

'F)P'[=1  'STEP  '1  'IJM  TIL  'MI  'DJ  ' 

'F  JP  '1=  1  'STEP  '1  'UM  TIL  'P  'DJ  ' 

'BEGIM  '  Cf  I.  l)  =  n  r. 

'F  JR'X=1  'STEP 'I  'UM  TIL  'M  'DJ  ' 

C(  I  *  0  )  =C;.J  PYAC  r»X)*COPYRf  X,J)*C<  l.U)  * 

'EM  D  '  «• 

'EMP  '  MATRIXMULT  «• 
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'PROCEDURE  '  DUMPCA.MI,  M>  « 

'ARRAY  '  A  « 

'INTEGFR'  MI,M  * 

'REGIM  IMTEGEP'I.  )  e 

'FO  R  'I  =  1  'STEP  '1  'UNTIL  'MI  'DJ  ' 

'REGI  M  ' 

'FOR'.JM  'STFP  '1  'UMTIL  'M  'DO  ' 

WR!  TEC  1  (1.LAY  JUTC  'C  'S-M  DPDPD<.  PDD'>  ')  *  A(  I  »  .!  >  >  <r 
'  FM  D  '  « 

MFWLt  MC  1  0»  4)  * 

'EM  D  '  DIM  P  * 

'PROCEDURE'  CARTCOMV  * 

'REGI  M  ' 

OUTXC  1.  1  )=XC  1,  1  )*SIMC  X(  3.  1  >  >  ? 

ouTxca.  n=xc2»  i>*simcxc 3»  1  > >+xc  i.  n*xc  .*»  1  >*nsc xc  3»  1  > > 

OUTXC  3»  1)=XC  !»  1  >*CJSCXC  3»  1  )  >  « 

jutxc  At  i  >=xca»  i  >*cjscxc  3»  n  >-x<  i»  n*xc  a,  n* 

SIMCXC  3»  1 >  >  $ 

JUTiC  1»  1>=£C  It  1)*SIMCZC?»  1>)  <■ 

ouTic  a»  i  )=ic  i»  i  >*cosc£c?»  n  >  * 

'FM  0  '  CAPTCOMV  S 

'REAL  PROCEDURE '  RANDOM  * 

'8EGIM  INTEGER'  CALC-  * 

'COMMENT'  OLDX  MUST  BE  DECLARED  GLOBALLY  * 

ULDX  =  25S*ULDX  ? 

CALC=  EM  TI  ERC  JLDX  /  1  3 1  0  7 1  >  ? 

0LDX=ULD.X-  1  31  071  +  CALC  * 

RAM  DJM=  JL PX  ^  1  3 1  071  « 

'END'  RAM  p]M  f 

'PROCEDURE'  GAUSS  I  AM  C  MO  I  SE*  VA  RI  AMCE>  c 
'REAL  MO  I SE» VAPI AN  CF  * 

'REGIM  REAL'  TWJPI  « 

TWO  PI  =  6.  P831B  ? 

U1 =  RAM  DJM  f 
U?=  RAM  DOM  «■ 

NJISF=SORTC-fH  VARIANCE*  L-MC  U1  >  )  *  CuSC  TW  PI  *  UP)  « 

'EM  D '  GAUSSIAM  * 

'PROCEDURE. '  SUBST  * 

'BEGIM  COMMENT'  TO  SFT  L  MATRIX  WM EM  MJISF  ADDED  * 

TOFT  AM  J I  SE=  RMO I  SE=  f  * 

A.\JC  RM  01  SE»  VAPR)  « 

GAIjSSI AMC  TMETAM  J!  SE»  V'ARTHETA)  « 

<CC  1  .  1  )  =  SOPTC  EX*F<*hY*'/Y>«-»NOl  ^F  * 

6(2,  \  )=AKCTAMC  FX/UY)*  THETAMOI  SF  F 
'FMD'  S'JRST  * 


CP6(,(T  iS> 


'PRJCFPIIPE'  TRACK  (  MEA  Fi  'RF*  $  TA  TE*  PH  I » M»  MT»  P»  X*  Z *  P)  * 

'ARRAY  '  PHI ,  P.  X,  '»  P,MT»M  «“ 

'INTEGER  '  MEASURE*  STATE  * 

'RFRIN  ARRAY'  'I  PC  1  SMFAFURF*  1 :  FTATF)  *  PH  I  TC  1  s  STA  TE»  1 :  STA TE> » 

MPMTC  1 J  MFAFIJRF:*  1  t  MEA  SURE) »  PREP-ZC  1  s  MEASURE*  I:  1  >» 

FRRORCJVC  lsMEASURF.  1 :  MEASURE  >»  CORRECT  ION  C  1 J  STA  TE»  1  s  1  >  > 
km  pc  1 :  STATE*  1 :  TATE)  « 

TRAN  $23  SEC  PHI*  PHI  T*  A,  A)  r 
MATRI  XMIJLTC  PHt  »  X,  X,  A,  A,  1)  T 
MATPIXMULTC  p.  phi  t.  p.  A,  A,  A)  «■ 

MATRIXMiJLTC  PHI*  P»  P*  A,  A,  A)  « 

MATRI  XMl.LTCM,  P,MP*  2.  A,  A)  « 

MATRIXMULTC  P.MT*  PMT*  a,  a,  2>  «* 
mATR!  XMULTCM*  PMT*  MpMT*  2*  A,  2)  «• 

■VJATRI  XAnne  MPMT*  R.MPMT.  2.  2)  « 

MATRIXI'JVCMPMT*  PRRJRCJV*  P)  * 

MATRI  X.MMLTC  PMT*  EEPORCJ  V.K.  A,  2,  2)  * 

MATRI  XMULTCM.  X*  PREDZ  »  2.  A,  1  >  » 

MATRIX  SUHC  PREDZ »  C.  *  PRED/Z*  2*  1  >  * 

M A T ® I XM LL TC K ,  PRFPZ *  CORRECTION.  A,  2*  1  )  «• 

'COMMENT '  PPEDii  IS  NOW  Crt*X-Z)*K  « 

MATRI  X  ci  IRC  X  •  CORRECTION,  X  *  A*  J  )  * 

MAJPI  XMIJlTCK.MP.KMP,  A,  2*  A>  « 

MATPIX$IIRC  PjKMPi  P»  A*  A)  « 

'COMMENT'  FOR  A  A  VAPTARLE  STATE  V/FCT 1 RC  STA TF=  A)  , 

AN  D  A  TW3  V  A  R I  A  PL  P  M  PASUREMEN  T  VECTO  RC  M  FA  SURF=  2  >  * 

X  IS  A*  1  ,  7  I  S  2*  1  »  P  IS  A*  A*  M  is  2*  A*  PH  I  IF  A*  A,  K  IS  A«  P  * 

'END'  TRACK  « 

MC  1  .  1  )=MC  P»  3>=MTC  1  *  n=MTC  3*  2>-  1  15 

MCI,  2>=MC  1  »  3)*MC  1  »  A)=-MC  2,  1  )  =MC  2*  2)  =MC  2*  A)  =M  TC  1 , 2)  =M  TC  ?.  2)  =M  TC  2*  1  )  = 

MTC  3*  1  >=MTC  A,  l)=rtTC  a,  2>  =  n  «■ 

'F  JR'f=  1  'STEP  '1  'UNTIL  'A 'DO  ' 

'F  jf"  1=  1  'STEP  '1  'UN  TIL  'A  'DO  ' 

'RFGI N  ' 

PHIC  I.  I>  =  'IF  'I  'FC  'J  'THEN  'J  'ELSF'O  * 

PCI,.J>  =  0  9 
'END'  * 

S TA T E=  A  *  MEASURE=2  9 

'COMMENT'  STA  TP  VFCTJR  IS  A-  V/ARI ARLE*  MEASUREM  EN  T  VFCTJR 

is  p- variable  <r 

<0=  EF.ApC  Pp)  "  Y  0=  RFAOC  20  )  9 

XPJ  l  =  Pr:ACC  PO)  f  YDJT=READC  20)  9 

CHANGE  JF  XDJT=READC  20)  «“  CHAN  RE  JF  Y DO T=  PEAPC  20 )  * 

CMAM  RB=HEAnc  20)  «•  F  I  N  I  SH=  READC  20  )  9  PEL  TA  T=  RFA  DC  20  )  9 

VABR=  RFAOC  20)  *  VARTHETA=REAPC?0)  t  JL  PX=  REAPC  20  >  9 

NO  I  S(r=PcADC  20)  «■ 
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WRl  TETC  1  (1»  '(  (  'C  ')  '1ST'*  'ORDER  '*  'KALMAN  '(  '0  ')  )  ')  «■ 

initiate  track:  'tF'  noise  'eq'  i  'then' 

'BEGIN  ' 

GAUSS!  ANC  RNOT  SE,  VARR)  e 
GAUSSIAN f  THETAMOI  SF*  VARTHF.TA)  * 

'END'  'ELSE  '  RNO I  SE=  THETAMOI  SE~0  « 

COPYZC  1»  1  )  =  SQRTCXO*XfUYO*YimR'JOI  SE  f 
C0PYZC2,  1  )=ARCTAMCXO/YO)  +  THFTAMOI  SF  « 

CAR  TOO  MV  * 

DIJMPCOUU.  2,  t )  <r 
COPYRC  1»  1  >  =  RC  1.  1  )=  VARR  « 

CO  PY  RC  1 »  2  >  =  RC  1 »  2 )  =  CO  PY  RC  2,  1  >  =  RC2,  1  )  =  n  « 

CO  PYRC  2»  2)  =  RC  2*  2)  =  VARTH  ETA  < 

T=  DEL  TAT  * 

EX  =  XO+XDOT*  DELTAT  « 

WY=  Y  0+  Y DO  T*  DEL  TAT  * 

'I  F  'NO I  SE  'EO  '1  'THEM  '  SU«ST  « 

'I  F  'NOISE  'EO  '0  'THEM  ' 

'BEGIN  ' 

£.(.  1,  1  )  =  $QRTC  EX*EX^UY*V.Y)  * 

'C 2,  1 )  =  APCTANC EX' wY)  « 

'EM  D  ' 

T I N  E=  T  * 

xc  i»  i )  =  <£c  i.  n  * 

XC  2,  1  )=C  id,  \  )  -COPYRC  1,1)  )' DEL  TAT  * 

/C  3.  1  >=<£C  2.  1  )  « 

XC  A,  1  )  =  C  XC  2»  1  )  “CO  PYXC  2»  1  )  )  /  DEL  TA T  * 

PC  1 ,  1  )  =  PC  1 ,  1  )  * 

PC  1,2)  =  PC  2,  1)  =  RC  1.  1) 'DELTAT  * 

PC  2.  ?)=C  RC  1,  1 )  ♦  COPY  PC  1 ,  1  )  )  /(,  DEL  TA  T*  DFL  TA  T)  ? 

PC  3*  3 )  =  RC  2,  2)  * 

PC  3,  A)  =  pc  4,  3)  =  RC  2,  ?)/ DElTAT  « 

PC  4.  4)  =  C  RC  2»  2)+-C0PYRC  2*  2)  )/<  DFlTAT*  DELTAT)  * 

'COMM  E-M  T  '  TRACK  IS  MJW  INITIATED  1 

CARTCOMV  «■ 

DWPCOUTX,  4,  1  )  t 
DUMPCOUTX.  2,  1  )  c 

'F  JR'T=2t  DEL  TAT 'STEP 'DEL  TAT 'UNTIL  'CHAM  GF  'D  J  ' 

'REGIN  ' 

EX=XnMnOT*T  * 
wY--Yfl+-YDJ  Tk  T  «■ 

'I  F  'M  1 1  SF  'EO  'j  'THEN  '  SURST  9 
'I  F  'i  JI  SE  'EO  '0  'THEM  ' 

'RFGTN  ' 

XC  1 ,  1  )=  SORTC  RX*  EX  +  IvY*  V.Y)  * 

A  C  2,  1  )  =  ARCTAMC  F.X  '  WY  )  9 
'END'  «• 

'COMMFNT'  NOISE  ADDED,  tol  TH  VARf  AMCE  VARR  AND  5  AR  THETA 
IN  THE  TLO  PRECEDING  STATEMENTS  9 
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PH  I  C  1»2)  =  PHIC3»  A)  =  DEL  TA  T  < 

TRACK <  P»  A,  PHI  P*  X»  L,  R)  * 

CARTCJMV  « 

DIM  PC  0 IITX  •  <4»  1  )  « 

DlMPCJUTZ*  2*  1  )  « 

TIME=T  « 

'EMD'  « 

xo=xn«-xmi*n.*iE  «“ 

YO=Yn«-YDJT*TIME  * 
xr»JT=<DJT> CHANGE  JF  XWT  * 

Yf>JT=YWT«- CHANGE  3F  YDOT  * 

FIMISH=FIMISH-TIME  * 

'F  3  R'T=  DEL  TAT  'STEP  'PEL  TAT'UM  TIL  'FTMI  H  'P3  ' 

'PFGI  M  ' 

FX  =  X  n+X  PJT*  T  «“ 

WY=YO+ YT»  T*  T  * 

'I  F  'MO!  SE  'EQ  '1  'THEM  'Sl'«$T  * 

'l  F  'M  J  I  SE  'EQ  '0  'THEM  ' 

'«EGI  M  ' 

Li  1.  I  )  =  <?ORTCEX*EX+  WY*WY)  * 

Li  P.  1  >=ARCTAMC  EX/ VCY>  r 

/  fT^  p  /  «■ 

'COMi'lEMT'  M- J I SF  APDFP.  W£  TH  VAPP  AMP  VAP7HFTA  TM  HE 
TV.*)  PRECEPT  MG  STATFiMEM  TS  * 

PH  T  C  1  *  ?)  =  PH  I  (  3»  A)  =  PEu  TAT  * 

TPACkCP.  a,  PHI.M.^T.  P.X.d.  R>  * 

CAPTCJMV/  « 

PIMPOJI'TX#  A,  1 )  "■ 

DIM  PC  JUT£.  P.  n  * 

'EM  P  '  c 
'EMP  ' 
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♦  IODUNITIO/PRINT 

♦  IQ  DUN  I  T20/READ 

♦  XEQ 

♦  STORAGE/ 4000/ 1  5000 

♦  CHAIN  1 

♦  ALGOL 

'BEGIN  INTEGER'  I ,  J,  MEASURE*  STA  TE»  Gl  DX»  F? 

'ARRAY'  XC  1:  6,  !:  1>,ZC  1:2,  l:  1>»GLDZC  1:2,  1:  1), 

OLDESTZC  1:2,  l:  1),  PHIC  l:  6,  l:  6),  PC  l:  6,  l:  6), 

R(  1  s  2»  1  s  2 )  *  OL DRC  1:2,  1 :  2),GlDESTRC  1*2,  1:2), 

MC  1:2*  IS  6>»MTC  IS  6,  l:2),GUTXC  1:  4*  1:  1)»GUTZ(1«  2»  !S  1  >  S 
'REAL'  T,  TIME,  DELTAT,  CHANGE,  FINISH,  EX,  XDGT»  CHANGE  OF  XDOT 
WY,  YP3T,  CHANGE  OF  YOG  T»  X 0»  Y 0,  VARR,  VARTH  ETA,  DEL  TATSQ, 

RNO I  SE*  THETANOISE*  Ul#  U2*  VARI  ANCE,NOl  SE  « 

'COMMENT'  X,Z  AND  OLDZ  ARE  COLUMN  VECTORS* 

R  AND  OLDR  ARE  2+2  MEASUREMENT  COVARIANCE  MATRICES* 

K  ISA  6*2  MATRIX*  AND  IS  ESSENTIALLY  THE  DAMPING  FACTOR* 
M  IS  THE  MEASUREMENT  MATRIX  AND  IS  2^6* 

MT  IS  THE  TRANSFUSE  OF  M. 

TRACK  IN  R,  THETA,  ROOT,  THETADOT,  RDOUBLEDOT*  THETA  DO  URL  EDIT* 
GENERATED  IN  X,  Y  WITH  CONSTANT  DATA  RATE* 

MANOEUVRE  AT  TIME  CHANGE*  FINISH  AT  TIME  FINISH* 

KALMAN  FILTER  TRACKER  WITHOUT  PLANT  NOISE  TERM S  * 

'PROCEDURE'  TRAN SPUSEC  PHI*  PHI  T»MI»N)  * 

'INTEGER'  MI,N  S 
'ARRAY  '  PHI,  FHIT  ? 

'BEGIN  INTEGER'  I»J  * 

'ARRAY  '  CdPYFHIC  isrtl,  1*N>  ? 

'FOR '1=1  'STEP  '1  'UNTIL  'Ml  'DO  ' 

'FUR'J=1  'STEP  '1  'UNTIL  'N  'DO  ' 

COPYPHI C  r,  J)  =  PHI(  I,  J>  S' 

'COMMENT'  PHI  COPIED  SO  THAT  TRANSPOSE  CAN  BE  ASSIGNED 
TO  ORIGINAL  MATRIX  (ONLY  POSSIBLE  FOR  SQUARE  MATRICES)  ’ 
'FOR '1=1  'STEP  '1  'UNTIL  'MI  'DO  ' 

'«OR'.J=  1  'STEP  '1  'UN TIL  'N  'DO  ' 

PHI  T(J,.I>  =  PH!  CI*T>  * 

'END 'TRANSPOSE  « 

'PROCEDURE'  MATRIXIN  VC  A,  B* MI  )  S 
ARRAY  '  A*  B  « 

'INTEGER'  MI  S 
'BEGIN  REAL '  DET  S 
'COMMENT'  TO  INVERT  A  2^2  MATRIX  S 
'IF  'MI  'N E  '2  'THEN  BEGKv  ' 

'  WRITETC  Id,  'CC  'C'>  m  NE  2')')  F 
'GO  TJ  '  STOP  <’ 

'END'  «  ‘ 

DET=AC  1 .  1J^AC2*2)-AC  !*2)  +  AC2*  1)  S 
BC  1,  1)=A(  2,  2) /DET  « 

BC 1, 2>  =  C-A(  I,  2>/DET)  < 

BC  2,  1 >  =  ( -AC  2*  1 > / DET)  « 

BC  2*  2)  =  AC  1,  1  )  /DET  c 
STOP:  'END'  MATRIXINV  * 
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'PROCEDURE'  MATRIXADPC  A>  0,  C»M!»N>  $ 

'ARRAY'  A»B»C  * 

'INTEGER'  MI »N  « 

'COMMEN  T '  THE  SUM  OF  THE  MATRICES  IS  STORED  IN  THE  ARRAY 
C.A,  B»  C  ARE  MI*N  $ 

'BEGIN  INTEGER'  l.J.P  9 

'FG R '1=  1  'STEP  '1  'UNTIL  'MI  'DO  ' 

'FUR  'J=  1  'STEP  '1  'UNTIL  'N  '03  ' 

CC  I.J>  =  A<  I.  J)  +  BH»  J>  ♦  ' 

'END'  MATRIXADD  ? 

'PROCEDURE  '  MATRIX SUB(  A*  B»  C» M I  »N  )  S 
'ARRAY  '  A*  PiC  ? 

'INTEGER'  MI»N  « 

'COMMENT  THE  DIFFERENE  OF  THE  TWO  MATRICES  IS  STORED  IN  TOE 
ARRAY  C.  A»R.  C  ARE  Ml*.N  « 

'BEGIN  INTEGER'  I.J  « 

'FOR '1=1  'STEP '1  'UNTIL 'Ml 'DU  ' 

'PJR'.J=  1  'STEP  '1  'UNTIL  'N  'DU  ' 

C(I»J)  =  A(I»J)-B(I>J)  t 
'END'  MATRIX  SUB  S 

'PROCEDURE'  MATRIXMULTC  A»  8»  C»  MI»N»  P)  ? 

'ARRAY  '  A»  B*  C  « 

'INTEGER'  MI  »N*  P  « 

'COMMENT'  TOE  PRODUCT  OF  TOE  TWO  ARRAYS  IS  STORED  IN  TOE 

ARRAY  C.  *TU  MULTIPLY  BY  A  SCALAR  TOE  SECOND 

MATRIX  MUST  HAVE  THE  VALUE  OF  THE  SCALAR  AS  ITS  DIAGONAL 

WITH  THE  REST  OF  TOE  ELEMENTS  AS  N0UGTT3.  A  IS  M*N,P  £S  N*P» 

C  IS  M*P.  COPIES  OF  INPUT  ARRAYS  ARE  MADE  £N  ORDER 

THAT  AN  ARRAY  MAY  BE  USED  BOTH  AS  AN  INPUT  AND 

OUTPUT  PARAMETER  ? 

'REGIN  INTEGER'  l»  J*  X  « 

'ARRAY  '  CQPYAC  1SMI,  lt.N)«COPYBC  !SN»  1?  P)  * 

'FOR  '1=  1  'STEP  '1  'UNTIL  'MI  'DO  ' 

'F0R'J=1  'STEP  '1  'UNTIL  'N  '[>3  ' 

CO  PYA(  t».J>  =  A(I»J>  5 

'FOR '1=1  'STEP  '1  'UNTIL  'N  -'5X1  ' 

'FOR 'J=  1  'STEP 't  'UNTIL  'P'Di  ' 

CJPY8<  I»J)  =  R\IpJ>  * 

'FJR'I  =  1  'STEP  '1  'UNTIL  'Ml  'DO  ' 

'F0R'J=1  'STEP  '1  'UNTIL  'P'CO  ' 

'PEGfN  '  CCI».J)  =  I)  y 

'FO R  'X=  1  'S TEP  '  1  'UN  Ti L  'N  'F>0  ' 

C(  i.  J>  =  COPYA(  r>X)f  CJPYBCX,  J>*C(UO>  < 

'END'  S 

'END'  M.ATRI XMULT  ? 
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'PROCEDURE  '  DUMPCA tMI.N)  ¥ 

'ARRAY  '  A  * 

'INTEGER'  MI»N  « 

■'BEGIN  INTEGER  "I  »J  * 

'FOR '1=1  'STEP  '1  'UNTIL  'Ml  'DO  ' 

'’BEGIN  '  ‘ 

'FOR  'J=  1  'STEP  '1  'IN TIL  'N  'DO  ' 

WRI  TEC  1  0>  LAYOUT!  'C  'S-NDDDDD.  ODD')  ') »  AC  I »  J  >  > 
'END'  « 

NEWLINC  1  0»  4)  ¥ 

'END'  DUMP  ¥ 

'PROCEDURE'  CONVERT  ¥ 

'BEGIN  ' 

G  UTX  (  1  #  1  >  *  X  C  1 »  1  >  *  $  I N  (  X  C  4*  1  >  >  ¥ 

OUTXC  ?»  1  )=XC2t  1  >*SINCXC  4#  1  >  >*XC  1#  1>*XC  5»  1  > 

*  CO  SC  X  C  4#  1  >  >  « 

0  (ITXC  3»  1  )  =  XC  1 »  1  >*CO  SC  XC  4*  1  >  >  « 

OUTXC  4»  1  )=XC?»  1  >*CO  SC  XC  4j  1  >  >-XC  1»  1>*XC  5»  I  ) 
*SINCXC4fl)>  ¥ 

OIJTZC  it  1  >=ZC  1*  1)*SINCZC2#  1)>  ¥ 

0UT2C2,  1)=ZC  1»  1>*CJSCZC2#  1>>  ¥ 

'END'  CONVERT  « 

'REAL  PROCEDURE '  RANDOM  ¥ 

'BEGIN  INTEGER'  CALC  « 

''COMMENT'  OLDX  MUST  BE  DECLARED  GLOBALLY  ¥ 
0LDX=255*0LDX  ¥ 

CALC=ENTIERCOLDX/131071)  * 
dLDX=OLDX-131071*CALC  ¥ 

PAND3M=OLDX/131 0  71  « 

'END'  RANDOM  ? 

'PROCEDURE'  GAUSSI ANC  NO  I  SE»  VARIANCE)  ¥ 

'REAL  '  NO  I  SEt  VARIANCE  ¥ 

'BEGIN  REAL'  TWO  PI  ¥ 

TW0PI=6.2S31S  « 

Ul* RAN  DOM  ¥ 

U2=  RANDOM  ¥ 

NOISE-  SQRTC-2*  VARI  ANCE*LNC  U1  >  >*CGSC  TWO  PI*  U2>  " 
'END'  GAUSSIAN  ¥ 


'PROCEDURE'  SUBSTITUTE  ¥ 

'BEGIN  COMMENT'  W  SET  2  MATRIX  WHEN 
THETANOI  SE=RNOI  SE=  0  ¥ 

GAUSSIANC  RNJISE*  VARR)  ¥ 

GAUSSIANC  THETANOI  SE»  VARTHETA)  ¥ 

ZC  1»  1  )  =  S9RTC  EX*EX«-VY*WY)  +  RNOISE¥ 
ZC2»  1  >  =  ARC  TAN  C  EX/  WY>  +  THE  TANG  I  SE  ¥ 
'END'  SUBSTITUTE  ¥ 


MATRIX  WHEN  NOISE  ADDED 
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'PROCEDURE'  TRACKCMFASURE#  STATE*  PHI  »M»MT»  P»  X*  Z#  P>  « 

'ARRAY'  PHI#  P*X»Z*  R.MT#M  « 

'INTEGER'  MEASURE#  STATE  f 

'BEGIN  ARRAY'  MPC  t : MEASURE.  1 :  STA  TE)  »  PH  I  TC  1 :  STATE#  1 :  STATE)  » 

MPMTC  1 : M EASURE#  l:MEASURE)»  PREDZC  UMEASURE#  Is  1 )» 

ERRORCUVC  1  t  MEASURE#  l  SMEASURF)  »  CO  RRECTIONC  l:  STATE.  1:  1  )# 

KM  PC  l:  STATE#  IS  STATE),  PMTC  IS  STATE.  1 :  MEASURE)  #  KC  IS  STATE#  1 :  M  EASURE)  f 
TRAN  SPO  SEC  PHI#  PH  IT#  STATE#  STATE)  * 

M  ATRIXMll.  TC  PH  I »  X#  X*  STATE#  STATE#  1  )  * 

MATRI  XMULTC  P#  PHI  T#  P#  STATE#  STATE#  STATE)  $ 

MATRIXMULTC  PHI#  P#  P#  STATE#  STATE.  STATE)  « 

MATRI XMULTCM.  P.MP.MEASURE#  STATE#  STATE)  « 

MATRIXMULTC  P#MT#  PM T»  STATE.  STATE#  MEASURE)  « 

MATPIXMIJLTCM#  PM T# M PM T# M EA SURE#  STATE# MEASURE)  * 

MATRI XADDC M PM T#  R# M PM T» MEASURE# MEASURE)  ? 

MATRIXINVCMPMT#  ERRJ  RCO  V.  MEASURE)  « 

MATRIXMULTC  PM T#  ERRJRCO  V.K#  STA TE.MEASURE#  MEASURE)  ? 
MATRIXMULTCM.X#  PREDZ# MEASURE#  STATE#  1)  S 
MATRIXSURC  PREDZ.Z#  PREDZ# MEASURE#  1)  « 

MATRI  XMUL  TCK#  PREDZ#  CORRECTION*  STA  TE.MEASURE#  1)  « 

'COMMENT'  PREDZ  IS  NOW  CM*X-Z)*K  $ 

MATRIXSUBCX,  CORRECTION,  X,  STATE.  1>  ? 

MATRI XMULTCK.MP.KMP#  STATE. MEASURE#  STATE)  ? 

MATRI XSUBC  P.KMP#  P,  STATE#  STATE)  5 

'COMM HINT'  FOR  A  6  VARIABLE  STATE  VECTURC  STATE=6)» 

AND  A  TWO  VARIABLE  MEASUREMENT  VECTO  RCM  EASURE*  5)  # 

X  IS  6*1#Z  IS  5*1#  P  IS  6*6#  M  IS  8*6#  PHI  IS  6*6#K  IS  6*5  * 
'END'  TRACK  S 

'COMMENT'  SETTING  OF  MEASUREMENT  MATRIX  S 
'FOR '1*1#  5'D.)  ' 

'FOR'J=l  'STEP  '1  'UNTIL  '6 'DO  ' 

MC  I#.J)=MTC  J»  I  )=0  « 

MC  1 »  1  >=MTC  1#  1  )=MC5»  4)=MTC  4#  ?)=  1  «s 

'COMMENT'  SETTING  TRANSITIONAL  MATRIX  AND  STATE  OJ.N VARI AN CE 
MATRIX  « 

'FOR '1=1  'STEP  '1  'UNTIL  '6'D3  ' 

'FOR '0=1  'STEP  '1  'UNTIL  '6'D3  ' 

'BEGIN  ' 

‘  PHIC  I#J)= 'IF 'I  'EQ'J 'THEN '1  'ELSE'O  « 

PCI#J)  =  0  « 

'EiND'  * 

STATE* 6  ?  MEASURE=2  * 

'COMMENT'  STATE  VECTOR  IS  6-  VARI  ABLE#  MEASUREMENT  VECTOR 
IS  8- VARI  ABLE  f 

X  0=  REAQC  80 )  *  Y  0=READ(  80 )  ? 

XDOT=REAPC  80)  *  YDO  T=  READC  50 )  9 

CHANGE  OF  XDJT=RFADC  8(1)  *  CHANGE  OF  YDOT=READC  50)  « 

CHANGE*  REA  DC  50)  *  FIN  I  SH=  READC  50)  «  DEL  TAT*  REA  DC  50)  S 
VARR=READC  80)  ?  VARTH ETA* READC  80)  *  OLDX=READC  50)  * 

DEL  TATSC1*DEL  TAT*  DEL  TAT  «  NO  I  SE=  READC  50)  * 
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WRI  TETC  1  0»  'C  C  'C  ')  '2N D  '*  'ORDER  '*  'KALMAN  ■'(  'C  ')  >  ')  * 

IN  HATE  TRACK:  'IF'  NOISE  'EQ  '  1  'THEN' 

'BEGIN  '  '  ... 

GAUSSIANC  RNUISE.  VARR)  « 

GAUSSIANC  THETANiJI  SE>  VARTHETA)  ? 

'END'  'ELSE'  RNO  I  S£=  TH  ETANO  I  SE=  0  P 
OLDESTZC  1*  1  )"=  SORTC X  Q*X  0+ Y  0*  Y0 )  +■  RNQ I  SE  « 

GLDESTEC2.  1)  =  ARCTANCX0/Y0)  +  THETANGI  SE  « 

C  jN  VERT  « 

DUMPC0UTZ.2,  1)  S 

F=LAYOUTC  '(  'S-iNDDOD.  DPDD')  ')  * 

WRI  TEC  1  0»LAYGUTC  '<  'S-NDD.  ODD')  ')»X0)  ? 

NEWLINC  1  0»  1 )  « 

WRI  TEC  1  0»  LAYO UTC  'C  'S-NDD*  DDD')  ')»  Y0>  S 
NEWLINC  1  0»  4)  ?‘ 

EX=X  0+XDU  T*  DEL  TAT  ? 

WY=YO+YDGT*DELTAT  « 

'IF 'NOISE  'EQ'  1  'THEN' 

'BEGIN  ' 

GAUSSIANC  RNGISE*  VARR)  P 

GA  US  SI  AN  C  TH  ETANO  I  SE*  VARTHETA)  « 

'END'  'ELSE'  RNG I  SE=  TH  ETANO  I  SE=  0  S 
GLDEC  1 »  1  )  =  SORTC  EX*  EX+  WY*  WY )  ♦  RNQ  I  SE  « 

0LDZC2,  1)  =  ARCTANCEX/WY)  +  THETA,NJISE  S 
EX=EX  +  XDOT*DELTAT  ? 

WY=  WY+ Y  DO  T*  DEL  TAT  « 

SUBSTITUTE  « 

PC  1,  l)=OLDRC  l»  l)=OLDESTRC  1»  1)  =  VARR  $ 

RC  2»  2)=0LDRC2»2)=0LDESTRC2,  2)=  VARTHETA  S 

RC  1»  2)  =  RC  2»  l)=OLDRC  1,  2)=uLDRC2*  !)=QLDESTRC  1 , 2)  =OL  DESTPC  2»  1>  =  0  f 
INITIATIONsXC  1*  1)  =  <£C  1»  1)  * 

XC2»  1 )  =  C  3*ZC  1»  1)-4*GLDZC  1»  D+OLDESTZC  1,  1 ) ) /C  2*  DEL  TAT)  « 

XC  3*  l)  =  COLDESTZC  1,  l)-P*OLnZC  1»  1)+ZC  1>  l))/DELTATSO  ? 

XC  4»  1>=ZC2»  1)  $ 

XC  5.  1  )  =  C  3*ZC  2»  1 )  -  4+OL  DZC  2»  1 ) +OL DESTZ C  2*  1 ) ) /C  P*  DEL  TAT)  ? 

XC  6,  1  )  =  C0LDESTZC2»  1)-2*0LDZC2#  1  )  +  ZC  2*  1)>/DELTATSQ  * 

'FUR '1=1 »  4'PJ  ' 

'BEGIN  ' 

J= 'IF'I 'EQ'l 'THEN '1 'ELSE'2  « 

PC  I »  I  )  =  RC  J » J  )  f 

PC  I»  1+  1  )=PC  1+  1»  I  )  =  3*RC  J»  J  )/C  DEL  TAT*  2)  ? 

PC  I*  I  +  ?)  =  PCI  +  2»  I)  =  RCJ*J)'f  DEL  TATSO  « 

PC  1  +  1»  i+ 1 )=CGLDESTRC J. J)+ 1  6*GL DRC  J»  J ) *-9*  RC  J»  J >  ) ' 

C  4*DEL  TATSO)  < 

PCI  +  2*I+1>  =  PCI+1»  I  +  2)  =  C  JL DESTRC  J»  J  )  +  3*0l DRC  J ) 

+  3*  RC  J  >  J  )  )  /(  2*  DEL  TATSQ*  DEL  TAT)  P 

PC  1  +  2*  I  +  2)  =  COL  DESTRC  J»  J  )  +  4+OLDRC  J»  J)  +  RCJ»  J)  )/ 

C  DELTA TSQ* DELTA TSG)  ? 

'END'  « 

‘  CONVERT  « 

DUMPCUUTX,  4»  1  )  « 

DUMPCOUTZ,  2.*  1 )  ? 

'COMMENT'  TRACK  IS  NOW  INITIATED:  TAKES  3  PLOTS 
TO  INITIATE  WITH  A  2ND  ORDER  TRACKER  S 


VS' >;  iyy^yvrorwB;  >v 
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GENERATION:  'FOR  'T=  3*  DEL  TAT  'STEP 'DEL  TAT 'IN  TIL  'CHAN  GE  'DO  / 
'REG IN  ' 

EX=X0+XD3T*T  9 
WY=YO+YDJT*T  S 
SUBSTITUTE  ? 

PHIC  1.2)  =  PHI(2>  3>  =  PH  I  (  4*  5)  =  PHI  (  5»  6)=  DEL  TAT  * 

PHIC  1.  3>  =  PHIC  4.  6>  =  DELTATSQ'2  « 

TRACK (  2»  6*  PHI #  M»  M  T»  P>  X. Z»  R>  5 
CONVERT  * 

DUMPCGUTX*  4>  1 )  9 
DUMPC  JUT^»  2*  1  )  f 
TIME=T  5 
'END'  S 

X0=XQ+X  DOT*  TIME  S 
Y0=  Y0+ YDO T* TIME  « 

X DO T=  X 00 T+ CHANGE  OF  XDOT  * 

YDOT=Y  DOT*  CHANGE  OF  YD3  T  f 
FIN  I  SH=  FINISH- TIME  ? 

'FOR'T=  DEL  TAT  'STEP  'DEL  TAT  'UN  TIL  'FIN  I  SH  'DO  ' 

'BEGIN  ' 

EX=XO+XDJT*T  « 

WY=YO+YDJT*  T  S 
SUBSTITUTE  f 

PHI  (  1»$)  =  PHI(2»3)=:PHI(  4*  5>  =  PHI  (  5.  6>  =  DEL  TAT  « 

PHIC  1.  3>  =  PHI(  4.  6)  =  DELT4TSQ/2  * 

TRACKC  2.  6*  PH  I .  M»  M  T»  P»  X*  2.  R)  « 

CONVERT  ? 

DUMPCGUTX.  4,  1  >  f 
DUMPCOUTZ.  2.  1)  $ 

'END'  * 

'END' 
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RESPONSE  TO  A  TURN  WITH  NOISE  FREE  DATA 


•  TRUE  POSITION,  VELOCITY 
+  POSITION  OR  VELOCITY  FROM  1st  ORDER  R-Q  TRACKER 

0  POSITION  OR  VELOCITY  FROM  2nd  ORDER  R-Q  TRACKER 

X  POSITION  OR  VELOCITY  FROM  X*Y  TRACKER 

DATA  INTERVALS  S  TIME  UNITS 
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OF  X 


V 


TRUE  VALUE 
OF  V 


3  b  1  B  9  10  II 
LOT  NUMBER 


.•  DISTANCE  UNITS 


SECOND  ORDER  POLAR  KALMAN  FILTER 


INPUT  POSITIONS  CROSSING  DISTANCE  =10 

SMOOTHED  POSITIONS  DATA  INTERVAL  *  5  TIME  UNITS 


9  _1 


8  1 
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b 
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I  I 
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|  | 
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A.S.W.E , 


START 
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- 1 - 1 

9  10 

DISTANCE  UNITS 


PLOT  NUMBER 


TRUE  VALUE 
OF  X 


PLOT  NUMBER. 


TRUE 
"  OF 


VALUE 

Y 


DATE.  i|  t  Tt. 


tr^ach.. 


DRG,  No. 
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X  Y  KALMAN  CRACKER  (^FIRST  ORDER) 


"• —  -  iN!=U~  kATA 

X  *  OLTPuT 

CROSSING)  DISTANCE  -lO 
DATA  INTERVAL  -  5  TIME.  U  NITS 


(TRUE  VALUE 
OF  *) 


(true  value-I 
OF  -V) 
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DATE  lt+  7t  TR^/I4|^  .  „  ,  DRG  No. 


A-S.W.E 


FIRST  OilDER  CARTESIAN  TRACKER 


CROSSING  DISTANCE  =  30 
DATA  INTERVAL  =  3  TIME  UNITS 

INPUT 

_ x _  SMOOTHED 


TRUE  X 

* 


START 


■A  I 


INCREASING  TIME 
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•'-l 
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fv 
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- AaL*^*** 
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W3L- 

TRUE  X 


4  6  8  10  tl  If  It 

PLOT  No¬ 


te  22  2f  26  2# 


“l’06' 


-l'04- 


-1-02- 


A***v 


-100- - X - 

TRUE  Y 


-0-06-1  * 


-0-94- 


-0*92- 


-0*90- 


-0-88  Hx 


8  IO  1*1416  ifctttt  M  26  28 

PLOT  No-«- 


FIRST  ORDER  POLAR  KALMAN 


CROSSING  01STANGE  =  30 
DATA  INTERVAL  =  3  TIME  UNITS 

— —  ■  ■  INPUT 
-  -x —  SMOOTHED 


2ND  ORDER  R0  KALMAN  TRACKER 


r~ 

8 


crossing  distance:  «  15 

DATA  INTERVAL  *  3  TIME  UNITS 
«-  -  INPUT 
X - SMOOTHED  , 


XY  KALMAN  TRACKER 


START 


\ 

: 


TRUE  VALUE  / 

.  _  1  _ 

X  /X"X-JSc 


V 


\  "  '^T-106- 

/  -1-04- 


TRUE  VALUE 

■  i  -  - 


?  >  -100- 
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3  A  STANDARD  DEVm  FOR  PLOTS 


SJM£.  DATE»4*TR^CH  A PP 


DRG  No 


SECOND  ORDER  R0  KALMAN  TRACKER 
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